View Javadoc
1   package com.irurueta.navigation.inertial.calibration;
2   
3   import com.irurueta.algebra.Matrix;
4   import com.irurueta.algebra.WrongSizeException;
5   import com.irurueta.navigation.LockedException;
6   import com.irurueta.navigation.NotReadyException;
7   import com.irurueta.navigation.frames.CoordinateTransformation;
8   import com.irurueta.navigation.frames.ECEFFrame;
9   import com.irurueta.navigation.frames.FrameType;
10  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11  import com.irurueta.navigation.frames.NEDFrame;
12  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14  import com.irurueta.navigation.geodesic.Constants;
15  import com.irurueta.navigation.inertial.BodyKinematics;
16  import com.irurueta.navigation.inertial.ECEFGravity;
17  import com.irurueta.navigation.inertial.ECEFPosition;
18  import com.irurueta.navigation.inertial.ECEFVelocity;
19  import com.irurueta.navigation.inertial.NEDPosition;
20  import com.irurueta.navigation.inertial.NEDVelocity;
21  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
22  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
23  import com.irurueta.statistics.UniformRandomizer;
24  import com.irurueta.units.Acceleration;
25  import com.irurueta.units.AccelerationUnit;
26  import org.junit.Ignore;
27  import org.junit.Test;
28  
29  import java.util.ArrayList;
30  import java.util.Collection;
31  import java.util.Collections;
32  import java.util.List;
33  import java.util.Random;
34  
35  import static org.junit.Assert.*;
36  
37  public class KnownPositionAccelerometerCalibratorTest implements
38          KnownPositionAccelerometerCalibratorListener {
39  
40      private static final double TIME_INTERVAL_SECONDS = 0.02;
41  
42      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
43      private static final double DEG_TO_RAD = 0.01745329252;
44  
45      private static final double MIN_ANGLE_DEGREES = -180.0;
46      private static final double MAX_ANGLE_DEGREES = 180.0;
47  
48      private static final double MIN_LATITUDE_DEGREES = -90.0;
49      private static final double MAX_LATITUDE_DEGREEs = 90.0;
50      private static final double MIN_LONGITUDE_DEGREES = -180.0;
51      private static final double MAX_LONGITUDE_DEGREES = 180.0;
52      private static final double MIN_HEIGHT = -50.0;
53      private static final double MAX_HEIGHT = 50.0;
54  
55      private static final int LARGE_MEASUREMENT_NUMBER = 100000;
56  
57      private static final double ABSOLUTE_ERROR = 1e-8;
58      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
59  
60      private static final int TIMES = 100;
61  
62      private int mCalibrateStart;
63      private int mCalibrateEnd;
64  
65      @Test
66      public void testConstructor1() throws WrongSizeException {
67          final KnownPositionAccelerometerCalibrator calibrator =
68                  new KnownPositionAccelerometerCalibrator();
69  
70          // check default values
71          assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
72          assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
73          assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
74          final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
75          assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
76          assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
77          final Acceleration bx2 = new Acceleration(0.0,
78                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
79          calibrator.getInitialBiasXAsAcceleration(bx2);
80          assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
81          assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
82          final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
83          assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
84          assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
85          final Acceleration by2 = new Acceleration(0.0,
86                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
87          calibrator.getInitialBiasYAsAcceleration(by2);
88          assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
89          assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
90          final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
91          assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
92          assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
93          final Acceleration bz2 = new Acceleration(0.0,
94                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
95          calibrator.getInitialBiasZAsAcceleration(bz2);
96          assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
97          assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
98          assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
99          assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
100         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
101         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
102         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
103         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
104         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
105         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
106         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
107         final double[] bias1 = calibrator.getInitialBias();
108         assertArrayEquals(bias1, new double[3], 0.0);
109         final double[] bias2 = new double[3];
110         calibrator.getInitialBias(bias2);
111         assertArrayEquals(bias1, bias2, 0.0);
112         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
113         assertEquals(b1, new Matrix(3, 1));
114         final Matrix b2 = new Matrix(3, 1);
115         calibrator.getInitialBiasAsMatrix(b2);
116         assertEquals(b1, b2);
117         final Matrix ma1 = calibrator.getInitialMa();
118         assertEquals(ma1, new Matrix(3, 3));
119         final Matrix ma2 = new Matrix(3, 3);
120         calibrator.getInitialMa(ma2);
121         assertEquals(ma1, ma2);
122         assertNull(calibrator.getEcefPosition());
123         assertNull(calibrator.getNedPosition());
124         assertFalse(calibrator.getNedPosition(null));
125         assertNull(calibrator.getMeasurements());
126         assertFalse(calibrator.isCommonAxisUsed());
127         assertNull(calibrator.getListener());
128         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
129         assertFalse(calibrator.isReady());
130         assertFalse(calibrator.isRunning());
131         assertNull(calibrator.getEstimatedBiases());
132         assertFalse(calibrator.getEstimatedBiases(null));
133         assertNull(calibrator.getEstimatedBiasesAsMatrix());
134         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
135         assertNull(calibrator.getEstimatedBiasFx());
136         assertNull(calibrator.getEstimatedBiasFy());
137         assertNull(calibrator.getEstimatedBiasFz());
138         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
139         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
140         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
141         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
142         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
143         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
144         assertNull(calibrator.getEstimatedMa());
145         assertNull(calibrator.getEstimatedSx());
146         assertNull(calibrator.getEstimatedSy());
147         assertNull(calibrator.getEstimatedSz());
148         assertNull(calibrator.getEstimatedMxy());
149         assertNull(calibrator.getEstimatedMxz());
150         assertNull(calibrator.getEstimatedMyx());
151         assertNull(calibrator.getEstimatedMyz());
152         assertNull(calibrator.getEstimatedMzx());
153         assertNull(calibrator.getEstimatedMzy());
154         assertNull(calibrator.getEstimatedCovariance());
155         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
156     }
157 
158     @Test
159     public void testConstructor2() throws WrongSizeException {
160         final KnownPositionAccelerometerCalibrator calibrator =
161                 new KnownPositionAccelerometerCalibrator(this);
162 
163         // check default values
164         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
165         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
166         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
167         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
168         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
169         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
170         final Acceleration bx2 = new Acceleration(0.0,
171                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
172         calibrator.getInitialBiasXAsAcceleration(bx2);
173         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
174         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
175         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
176         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
177         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
178         final Acceleration by2 = new Acceleration(0.0,
179                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
180         calibrator.getInitialBiasYAsAcceleration(by2);
181         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
182         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
183         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
184         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
185         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
186         final Acceleration bz2 = new Acceleration(0.0,
187                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
188         calibrator.getInitialBiasZAsAcceleration(bz2);
189         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
190         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
191         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
192         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
193         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
194         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
195         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
196         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
197         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
198         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
199         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
200         final double[] bias1 = calibrator.getInitialBias();
201         assertArrayEquals(bias1, new double[3], 0.0);
202         final double[] bias2 = new double[3];
203         calibrator.getInitialBias(bias2);
204         assertArrayEquals(bias1, bias2, 0.0);
205         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
206         assertEquals(b1, new Matrix(3, 1));
207         final Matrix b2 = new Matrix(3, 1);
208         calibrator.getInitialBiasAsMatrix(b2);
209         assertEquals(b1, b2);
210         final Matrix ma1 = calibrator.getInitialMa();
211         assertEquals(ma1, new Matrix(3, 3));
212         final Matrix ma2 = new Matrix(3, 3);
213         calibrator.getInitialMa(ma2);
214         assertEquals(ma1, ma2);
215         assertNull(calibrator.getEcefPosition());
216         assertNull(calibrator.getNedPosition());
217         assertFalse(calibrator.getNedPosition(null));
218         assertNull(calibrator.getMeasurements());
219         assertFalse(calibrator.isCommonAxisUsed());
220         assertSame(calibrator.getListener(), this);
221         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
222         assertFalse(calibrator.isReady());
223         assertFalse(calibrator.isRunning());
224         assertNull(calibrator.getEstimatedBiases());
225         assertFalse(calibrator.getEstimatedBiases(null));
226         assertNull(calibrator.getEstimatedBiasesAsMatrix());
227         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
228         assertNull(calibrator.getEstimatedBiasFx());
229         assertNull(calibrator.getEstimatedBiasFy());
230         assertNull(calibrator.getEstimatedBiasFz());
231         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
232         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
233         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
234         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
235         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
236         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
237         assertNull(calibrator.getEstimatedMa());
238         assertNull(calibrator.getEstimatedSx());
239         assertNull(calibrator.getEstimatedSy());
240         assertNull(calibrator.getEstimatedSz());
241         assertNull(calibrator.getEstimatedMxy());
242         assertNull(calibrator.getEstimatedMxz());
243         assertNull(calibrator.getEstimatedMyx());
244         assertNull(calibrator.getEstimatedMyz());
245         assertNull(calibrator.getEstimatedMzx());
246         assertNull(calibrator.getEstimatedMzy());
247         assertNull(calibrator.getEstimatedCovariance());
248         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
249     }
250 
251     @Test
252     public void testConstructor3() throws WrongSizeException {
253         final Collection<StandardDeviationBodyKinematics> measurements =
254                 Collections.emptyList();
255         final KnownPositionAccelerometerCalibrator calibrator =
256                 new KnownPositionAccelerometerCalibrator(measurements);
257 
258         // check default values
259         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
260         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
261         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
262         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
263         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
264         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
265         final Acceleration bx2 = new Acceleration(0.0,
266                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
267         calibrator.getInitialBiasXAsAcceleration(bx2);
268         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
269         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
270         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
271         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
272         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
273         final Acceleration by2 = new Acceleration(0.0,
274                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
275         calibrator.getInitialBiasYAsAcceleration(by2);
276         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
277         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
278         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
279         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
280         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
281         final Acceleration bz2 = new Acceleration(0.0,
282                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
283         calibrator.getInitialBiasZAsAcceleration(bz2);
284         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
285         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
286         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
287         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
288         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
289         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
290         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
291         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
292         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
293         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
294         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
295         final double[] bias1 = calibrator.getInitialBias();
296         assertArrayEquals(bias1, new double[3], 0.0);
297         final double[] bias2 = new double[3];
298         calibrator.getInitialBias(bias2);
299         assertArrayEquals(bias1, bias2, 0.0);
300         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
301         assertEquals(b1, new Matrix(3, 1));
302         final Matrix b2 = new Matrix(3, 1);
303         calibrator.getInitialBiasAsMatrix(b2);
304         assertEquals(b1, b2);
305         final Matrix ma1 = calibrator.getInitialMa();
306         assertEquals(ma1, new Matrix(3, 3));
307         final Matrix ma2 = new Matrix(3, 3);
308         calibrator.getInitialMa(ma2);
309         assertEquals(ma1, ma2);
310         assertNull(calibrator.getEcefPosition());
311         assertNull(calibrator.getNedPosition());
312         assertFalse(calibrator.getNedPosition(null));
313         assertSame(calibrator.getMeasurements(), measurements);
314         assertFalse(calibrator.isCommonAxisUsed());
315         assertNull(calibrator.getListener());
316         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
317         assertFalse(calibrator.isReady());
318         assertFalse(calibrator.isRunning());
319         assertNull(calibrator.getEstimatedBiases());
320         assertFalse(calibrator.getEstimatedBiases(null));
321         assertNull(calibrator.getEstimatedBiasesAsMatrix());
322         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
323         assertNull(calibrator.getEstimatedBiasFx());
324         assertNull(calibrator.getEstimatedBiasFy());
325         assertNull(calibrator.getEstimatedBiasFz());
326         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
327         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
328         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
329         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
330         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
331         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
332         assertNull(calibrator.getEstimatedMa());
333         assertNull(calibrator.getEstimatedSx());
334         assertNull(calibrator.getEstimatedSy());
335         assertNull(calibrator.getEstimatedSz());
336         assertNull(calibrator.getEstimatedMxy());
337         assertNull(calibrator.getEstimatedMxz());
338         assertNull(calibrator.getEstimatedMyx());
339         assertNull(calibrator.getEstimatedMyz());
340         assertNull(calibrator.getEstimatedMzx());
341         assertNull(calibrator.getEstimatedMzy());
342         assertNull(calibrator.getEstimatedCovariance());
343         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
344     }
345 
346     @Test
347     public void testConstructor4() throws WrongSizeException {
348         final Collection<StandardDeviationBodyKinematics> measurements =
349                 Collections.emptyList();
350         final KnownPositionAccelerometerCalibrator calibrator =
351                 new KnownPositionAccelerometerCalibrator(measurements, this);
352 
353         // check default values
354         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
355         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
356         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
357         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
358         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
359         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
360         final Acceleration bx2 = new Acceleration(0.0,
361                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
362         calibrator.getInitialBiasXAsAcceleration(bx2);
363         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
364         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
365         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
366         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
367         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
368         final Acceleration by2 = new Acceleration(0.0,
369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
370         calibrator.getInitialBiasYAsAcceleration(by2);
371         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
372         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
373         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
374         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
375         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
376         final Acceleration bz2 = new Acceleration(0.0,
377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
378         calibrator.getInitialBiasZAsAcceleration(bz2);
379         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
380         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
381         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
382         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
383         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
384         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
385         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
386         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
387         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
388         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
389         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
390         final double[] bias1 = calibrator.getInitialBias();
391         assertArrayEquals(bias1, new double[3], 0.0);
392         final double[] bias2 = new double[3];
393         calibrator.getInitialBias(bias2);
394         assertArrayEquals(bias1, bias2, 0.0);
395         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
396         assertEquals(b1, new Matrix(3, 1));
397         final Matrix b2 = new Matrix(3, 1);
398         calibrator.getInitialBiasAsMatrix(b2);
399         assertEquals(b1, b2);
400         final Matrix ma1 = calibrator.getInitialMa();
401         assertEquals(ma1, new Matrix(3, 3));
402         final Matrix ma2 = new Matrix(3, 3);
403         calibrator.getInitialMa(ma2);
404         assertEquals(ma1, ma2);
405         assertNull(calibrator.getEcefPosition());
406         assertNull(calibrator.getNedPosition());
407         assertFalse(calibrator.getNedPosition(null));
408         assertSame(calibrator.getMeasurements(), measurements);
409         assertFalse(calibrator.isCommonAxisUsed());
410         assertSame(calibrator.getListener(), this);
411         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
412         assertFalse(calibrator.isReady());
413         assertFalse(calibrator.isRunning());
414         assertNull(calibrator.getEstimatedBiases());
415         assertFalse(calibrator.getEstimatedBiases(null));
416         assertNull(calibrator.getEstimatedBiasesAsMatrix());
417         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
418         assertNull(calibrator.getEstimatedBiasFx());
419         assertNull(calibrator.getEstimatedBiasFy());
420         assertNull(calibrator.getEstimatedBiasFz());
421         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
422         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
423         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
424         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
425         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
426         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
427         assertNull(calibrator.getEstimatedMa());
428         assertNull(calibrator.getEstimatedSx());
429         assertNull(calibrator.getEstimatedSy());
430         assertNull(calibrator.getEstimatedSz());
431         assertNull(calibrator.getEstimatedMxy());
432         assertNull(calibrator.getEstimatedMxz());
433         assertNull(calibrator.getEstimatedMyx());
434         assertNull(calibrator.getEstimatedMyz());
435         assertNull(calibrator.getEstimatedMzx());
436         assertNull(calibrator.getEstimatedMzy());
437         assertNull(calibrator.getEstimatedCovariance());
438         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
439     }
440 
441     @Test
442     public void testConstructor5() throws WrongSizeException {
443         final KnownPositionAccelerometerCalibrator calibrator =
444                 new KnownPositionAccelerometerCalibrator(true);
445 
446         // check default values
447         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
448         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
449         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
450         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
451         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
452         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
453         final Acceleration bx2 = new Acceleration(0.0,
454                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
455         calibrator.getInitialBiasXAsAcceleration(bx2);
456         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
457         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
458         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
459         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
460         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
461         final Acceleration by2 = new Acceleration(0.0,
462                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
463         calibrator.getInitialBiasYAsAcceleration(by2);
464         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
465         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
466         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
467         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
468         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
469         final Acceleration bz2 = new Acceleration(0.0,
470                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
471         calibrator.getInitialBiasZAsAcceleration(bz2);
472         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
473         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
474         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
475         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
476         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
477         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
478         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
479         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
480         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
481         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
482         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
483         final double[] bias1 = calibrator.getInitialBias();
484         assertArrayEquals(bias1, new double[3], 0.0);
485         final double[] bias2 = new double[3];
486         calibrator.getInitialBias(bias2);
487         assertArrayEquals(bias1, bias2, 0.0);
488         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
489         assertEquals(b1, new Matrix(3, 1));
490         final Matrix b2 = new Matrix(3, 1);
491         calibrator.getInitialBiasAsMatrix(b2);
492         assertEquals(b1, b2);
493         final Matrix ma1 = calibrator.getInitialMa();
494         assertEquals(ma1, new Matrix(3, 3));
495         final Matrix ma2 = new Matrix(3, 3);
496         calibrator.getInitialMa(ma2);
497         assertEquals(ma1, ma2);
498         assertNull(calibrator.getEcefPosition());
499         assertNull(calibrator.getNedPosition());
500         assertFalse(calibrator.getNedPosition(null));
501         assertNull(calibrator.getMeasurements());
502         assertTrue(calibrator.isCommonAxisUsed());
503         assertNull(calibrator.getListener());
504         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
505         assertFalse(calibrator.isReady());
506         assertFalse(calibrator.isRunning());
507         assertNull(calibrator.getEstimatedBiases());
508         assertFalse(calibrator.getEstimatedBiases(null));
509         assertNull(calibrator.getEstimatedBiasesAsMatrix());
510         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
511         assertNull(calibrator.getEstimatedBiasFx());
512         assertNull(calibrator.getEstimatedBiasFy());
513         assertNull(calibrator.getEstimatedBiasFz());
514         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
515         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
516         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
517         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
518         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
519         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
520         assertNull(calibrator.getEstimatedMa());
521         assertNull(calibrator.getEstimatedSx());
522         assertNull(calibrator.getEstimatedSy());
523         assertNull(calibrator.getEstimatedSz());
524         assertNull(calibrator.getEstimatedMxy());
525         assertNull(calibrator.getEstimatedMxz());
526         assertNull(calibrator.getEstimatedMyx());
527         assertNull(calibrator.getEstimatedMyz());
528         assertNull(calibrator.getEstimatedMzx());
529         assertNull(calibrator.getEstimatedMzy());
530         assertNull(calibrator.getEstimatedCovariance());
531         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
532     }
533 
534     @Test
535     public void testConstructor6() throws WrongSizeException {
536         final KnownPositionAccelerometerCalibrator calibrator =
537                 new KnownPositionAccelerometerCalibrator(true,
538                         this);
539 
540         // check default values
541         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
542         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
543         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
544         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
545         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
547         final Acceleration bx2 = new Acceleration(0.0,
548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
549         calibrator.getInitialBiasXAsAcceleration(bx2);
550         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
552         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
553         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
555         final Acceleration by2 = new Acceleration(0.0,
556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
557         calibrator.getInitialBiasYAsAcceleration(by2);
558         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
560         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
561         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
563         final Acceleration bz2 = new Acceleration(0.0,
564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
565         calibrator.getInitialBiasZAsAcceleration(bz2);
566         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
568         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
569         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
570         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
571         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
572         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
573         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
574         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
575         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
576         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
577         final double[] bias1 = calibrator.getInitialBias();
578         assertArrayEquals(bias1, new double[3], 0.0);
579         final double[] bias2 = new double[3];
580         calibrator.getInitialBias(bias2);
581         assertArrayEquals(bias1, bias2, 0.0);
582         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
583         assertEquals(b1, new Matrix(3, 1));
584         final Matrix b2 = new Matrix(3, 1);
585         calibrator.getInitialBiasAsMatrix(b2);
586         assertEquals(b1, b2);
587         final Matrix ma1 = calibrator.getInitialMa();
588         assertEquals(ma1, new Matrix(3, 3));
589         final Matrix ma2 = new Matrix(3, 3);
590         calibrator.getInitialMa(ma2);
591         assertEquals(ma1, ma2);
592         assertNull(calibrator.getEcefPosition());
593         assertNull(calibrator.getNedPosition());
594         assertFalse(calibrator.getNedPosition(null));
595         assertNull(calibrator.getMeasurements());
596         assertTrue(calibrator.isCommonAxisUsed());
597         assertSame(calibrator.getListener(), this);
598         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
599         assertFalse(calibrator.isReady());
600         assertFalse(calibrator.isRunning());
601         assertNull(calibrator.getEstimatedBiases());
602         assertFalse(calibrator.getEstimatedBiases(null));
603         assertNull(calibrator.getEstimatedBiasesAsMatrix());
604         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
605         assertNull(calibrator.getEstimatedBiasFx());
606         assertNull(calibrator.getEstimatedBiasFy());
607         assertNull(calibrator.getEstimatedBiasFz());
608         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
609         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
610         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
611         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
612         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
613         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
614         assertNull(calibrator.getEstimatedMa());
615         assertNull(calibrator.getEstimatedSx());
616         assertNull(calibrator.getEstimatedSy());
617         assertNull(calibrator.getEstimatedSz());
618         assertNull(calibrator.getEstimatedMxy());
619         assertNull(calibrator.getEstimatedMxz());
620         assertNull(calibrator.getEstimatedMyx());
621         assertNull(calibrator.getEstimatedMyz());
622         assertNull(calibrator.getEstimatedMzx());
623         assertNull(calibrator.getEstimatedMzy());
624         assertNull(calibrator.getEstimatedCovariance());
625         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
626     }
627 
628     @Test
629     public void testConstructor7() throws WrongSizeException {
630         final Collection<StandardDeviationBodyKinematics> measurements =
631                 Collections.emptyList();
632         final KnownPositionAccelerometerCalibrator calibrator =
633                 new KnownPositionAccelerometerCalibrator(measurements,
634                         true);
635 
636         // check default values
637         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
638         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
639         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
640         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
641         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
642         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
643         final Acceleration bx2 = new Acceleration(0.0,
644                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
645         calibrator.getInitialBiasXAsAcceleration(bx2);
646         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
647         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
648         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
649         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
650         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
651         final Acceleration by2 = new Acceleration(0.0,
652                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
653         calibrator.getInitialBiasYAsAcceleration(by2);
654         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
655         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
656         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
657         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
658         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
659         final Acceleration bz2 = new Acceleration(0.0,
660                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
661         calibrator.getInitialBiasZAsAcceleration(bz2);
662         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
663         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
664         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
665         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
666         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
667         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
668         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
669         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
670         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
671         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
672         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
673         final double[] bias1 = calibrator.getInitialBias();
674         assertArrayEquals(bias1, new double[3], 0.0);
675         final double[] bias2 = new double[3];
676         calibrator.getInitialBias(bias2);
677         assertArrayEquals(bias1, bias2, 0.0);
678         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
679         assertEquals(b1, new Matrix(3, 1));
680         final Matrix b2 = new Matrix(3, 1);
681         calibrator.getInitialBiasAsMatrix(b2);
682         assertEquals(b1, b2);
683         final Matrix ma1 = calibrator.getInitialMa();
684         assertEquals(ma1, new Matrix(3, 3));
685         final Matrix ma2 = new Matrix(3, 3);
686         calibrator.getInitialMa(ma2);
687         assertEquals(ma1, ma2);
688         assertNull(calibrator.getEcefPosition());
689         assertNull(calibrator.getNedPosition());
690         assertFalse(calibrator.getNedPosition(null));
691         assertSame(calibrator.getMeasurements(), measurements);
692         assertTrue(calibrator.isCommonAxisUsed());
693         assertNull(calibrator.getListener());
694         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
695         assertFalse(calibrator.isReady());
696         assertFalse(calibrator.isRunning());
697         assertNull(calibrator.getEstimatedBiases());
698         assertFalse(calibrator.getEstimatedBiases(null));
699         assertNull(calibrator.getEstimatedBiasesAsMatrix());
700         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
701         assertNull(calibrator.getEstimatedBiasFx());
702         assertNull(calibrator.getEstimatedBiasFy());
703         assertNull(calibrator.getEstimatedBiasFz());
704         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
705         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
706         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
707         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
708         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
709         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
710         assertNull(calibrator.getEstimatedMa());
711         assertNull(calibrator.getEstimatedSx());
712         assertNull(calibrator.getEstimatedSy());
713         assertNull(calibrator.getEstimatedSz());
714         assertNull(calibrator.getEstimatedMxy());
715         assertNull(calibrator.getEstimatedMxz());
716         assertNull(calibrator.getEstimatedMyx());
717         assertNull(calibrator.getEstimatedMyz());
718         assertNull(calibrator.getEstimatedMzx());
719         assertNull(calibrator.getEstimatedMzy());
720         assertNull(calibrator.getEstimatedCovariance());
721         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
722     }
723 
724     @Test
725     public void testConstructor8() throws WrongSizeException {
726         final Collection<StandardDeviationBodyKinematics> measurements =
727                 Collections.emptyList();
728         final KnownPositionAccelerometerCalibrator calibrator =
729                 new KnownPositionAccelerometerCalibrator(measurements,
730                         true, this);
731 
732         // check default values
733         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
734         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
735         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
736         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
737         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
738         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
739         final Acceleration bx2 = new Acceleration(0.0,
740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
741         calibrator.getInitialBiasXAsAcceleration(bx2);
742         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
743         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
744         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
745         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
746         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
747         final Acceleration by2 = new Acceleration(0.0,
748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
749         calibrator.getInitialBiasYAsAcceleration(by2);
750         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
751         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
752         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
753         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
754         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
755         final Acceleration bz2 = new Acceleration(0.0,
756                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
757         calibrator.getInitialBiasZAsAcceleration(bz2);
758         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
759         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
760         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
761         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
762         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
763         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
764         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
765         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
766         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
767         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
768         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
769         final double[] bias1 = calibrator.getInitialBias();
770         assertArrayEquals(bias1, new double[3], 0.0);
771         final double[] bias2 = new double[3];
772         calibrator.getInitialBias(bias2);
773         assertArrayEquals(bias1, bias2, 0.0);
774         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
775         assertEquals(b1, new Matrix(3, 1));
776         final Matrix b2 = new Matrix(3, 1);
777         calibrator.getInitialBiasAsMatrix(b2);
778         assertEquals(b1, b2);
779         final Matrix ma1 = calibrator.getInitialMa();
780         assertEquals(ma1, new Matrix(3, 3));
781         final Matrix ma2 = new Matrix(3, 3);
782         calibrator.getInitialMa(ma2);
783         assertEquals(ma1, ma2);
784         assertNull(calibrator.getEcefPosition());
785         assertNull(calibrator.getNedPosition());
786         assertFalse(calibrator.getNedPosition(null));
787         assertSame(calibrator.getMeasurements(), measurements);
788         assertTrue(calibrator.isCommonAxisUsed());
789         assertSame(calibrator.getListener(), this);
790         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
791         assertFalse(calibrator.isReady());
792         assertFalse(calibrator.isRunning());
793         assertNull(calibrator.getEstimatedBiases());
794         assertFalse(calibrator.getEstimatedBiases(null));
795         assertNull(calibrator.getEstimatedBiasesAsMatrix());
796         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
797         assertNull(calibrator.getEstimatedBiasFx());
798         assertNull(calibrator.getEstimatedBiasFy());
799         assertNull(calibrator.getEstimatedBiasFz());
800         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
801         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
802         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
803         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
804         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
805         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
806         assertNull(calibrator.getEstimatedMa());
807         assertNull(calibrator.getEstimatedSx());
808         assertNull(calibrator.getEstimatedSy());
809         assertNull(calibrator.getEstimatedSz());
810         assertNull(calibrator.getEstimatedMxy());
811         assertNull(calibrator.getEstimatedMxz());
812         assertNull(calibrator.getEstimatedMyx());
813         assertNull(calibrator.getEstimatedMyz());
814         assertNull(calibrator.getEstimatedMzx());
815         assertNull(calibrator.getEstimatedMzy());
816         assertNull(calibrator.getEstimatedCovariance());
817         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
818     }
819 
820     @Test
821     public void testConstructor9() throws WrongSizeException {
822         final Matrix ba = generateBa();
823         final double biasX = ba.getElementAtIndex(0);
824         final double biasY = ba.getElementAtIndex(1);
825         final double biasZ = ba.getElementAtIndex(2);
826 
827         final KnownPositionAccelerometerCalibrator calibrator =
828                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
829 
830         // check default values
831         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
832         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
833         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
834         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
835         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
836         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
837         final Acceleration bx2 = new Acceleration(0.0,
838                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
839         calibrator.getInitialBiasXAsAcceleration(bx2);
840         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
841         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
842         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
843         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
844         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
845         final Acceleration by2 = new Acceleration(0.0,
846                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
847         calibrator.getInitialBiasYAsAcceleration(by2);
848         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
849         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
850         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
851         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
852         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
853         final Acceleration bz2 = new Acceleration(0.0,
854                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
855         calibrator.getInitialBiasZAsAcceleration(bz2);
856         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
857         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
858         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
859         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
860         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
861         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
862         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
863         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
864         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
865         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
866         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
867         final double[] bias1 = calibrator.getInitialBias();
868         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
869         final double[] bias2 = new double[3];
870         calibrator.getInitialBias(bias2);
871         assertArrayEquals(bias1, bias2, 0.0);
872         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
873         assertEquals(b1, ba);
874         final Matrix b2 = new Matrix(3, 1);
875         calibrator.getInitialBiasAsMatrix(b2);
876         assertEquals(b1, b2);
877         final Matrix ma1 = calibrator.getInitialMa();
878         assertEquals(ma1, new Matrix(3, 3));
879         final Matrix ma2 = new Matrix(3, 3);
880         calibrator.getInitialMa(ma2);
881         assertEquals(ma1, ma2);
882         assertNull(calibrator.getEcefPosition());
883         assertNull(calibrator.getNedPosition());
884         assertFalse(calibrator.getNedPosition(null));
885         assertNull(calibrator.getMeasurements());
886         assertFalse(calibrator.isCommonAxisUsed());
887         assertNull(calibrator.getListener());
888         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
889         assertFalse(calibrator.isReady());
890         assertFalse(calibrator.isRunning());
891         assertNull(calibrator.getEstimatedBiases());
892         assertFalse(calibrator.getEstimatedBiases(null));
893         assertNull(calibrator.getEstimatedBiasesAsMatrix());
894         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
895         assertNull(calibrator.getEstimatedBiasFx());
896         assertNull(calibrator.getEstimatedBiasFy());
897         assertNull(calibrator.getEstimatedBiasFz());
898         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
899         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
900         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
901         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
902         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
903         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
904         assertNull(calibrator.getEstimatedMa());
905         assertNull(calibrator.getEstimatedSx());
906         assertNull(calibrator.getEstimatedSy());
907         assertNull(calibrator.getEstimatedSz());
908         assertNull(calibrator.getEstimatedMxy());
909         assertNull(calibrator.getEstimatedMxz());
910         assertNull(calibrator.getEstimatedMyx());
911         assertNull(calibrator.getEstimatedMyz());
912         assertNull(calibrator.getEstimatedMzx());
913         assertNull(calibrator.getEstimatedMzy());
914         assertNull(calibrator.getEstimatedCovariance());
915         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
916     }
917 
918     @Test
919     public void testConstructor10() throws WrongSizeException {
920         final Matrix ba = generateBa();
921         final double biasX = ba.getElementAtIndex(0);
922         final double biasY = ba.getElementAtIndex(1);
923         final double biasZ = ba.getElementAtIndex(2);
924 
925         final KnownPositionAccelerometerCalibrator calibrator =
926                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
927                         this);
928 
929         // check default values
930         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
931         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
932         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
933         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
934         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
935         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
936         final Acceleration bx2 = new Acceleration(0.0,
937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
938         calibrator.getInitialBiasXAsAcceleration(bx2);
939         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
940         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
941         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
942         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
943         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
944         final Acceleration by2 = new Acceleration(0.0,
945                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
946         calibrator.getInitialBiasYAsAcceleration(by2);
947         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
948         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
949         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
950         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
951         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
952         final Acceleration bz2 = new Acceleration(0.0,
953                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
954         calibrator.getInitialBiasZAsAcceleration(bz2);
955         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
956         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
957         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
958         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
959         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
960         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
961         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
962         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
963         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
964         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
965         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
966         final double[] bias1 = calibrator.getInitialBias();
967         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
968         final double[] bias2 = new double[3];
969         calibrator.getInitialBias(bias2);
970         assertArrayEquals(bias1, bias2, 0.0);
971         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
972         assertEquals(b1, ba);
973         final Matrix b2 = new Matrix(3, 1);
974         calibrator.getInitialBiasAsMatrix(b2);
975         assertEquals(b1, b2);
976         final Matrix ma1 = calibrator.getInitialMa();
977         assertEquals(ma1, new Matrix(3, 3));
978         final Matrix ma2 = new Matrix(3, 3);
979         calibrator.getInitialMa(ma2);
980         assertEquals(ma1, ma2);
981         assertNull(calibrator.getEcefPosition());
982         assertNull(calibrator.getNedPosition());
983         assertFalse(calibrator.getNedPosition(null));
984         assertNull(calibrator.getMeasurements());
985         assertFalse(calibrator.isCommonAxisUsed());
986         assertSame(calibrator.getListener(), this);
987         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
988         assertFalse(calibrator.isReady());
989         assertFalse(calibrator.isRunning());
990         assertNull(calibrator.getEstimatedBiases());
991         assertFalse(calibrator.getEstimatedBiases(null));
992         assertNull(calibrator.getEstimatedBiasesAsMatrix());
993         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
994         assertNull(calibrator.getEstimatedBiasFx());
995         assertNull(calibrator.getEstimatedBiasFy());
996         assertNull(calibrator.getEstimatedBiasFz());
997         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
998         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
999         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1000         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1001         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1002         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1003         assertNull(calibrator.getEstimatedMa());
1004         assertNull(calibrator.getEstimatedSx());
1005         assertNull(calibrator.getEstimatedSy());
1006         assertNull(calibrator.getEstimatedSz());
1007         assertNull(calibrator.getEstimatedMxy());
1008         assertNull(calibrator.getEstimatedMxz());
1009         assertNull(calibrator.getEstimatedMyx());
1010         assertNull(calibrator.getEstimatedMyz());
1011         assertNull(calibrator.getEstimatedMzx());
1012         assertNull(calibrator.getEstimatedMzy());
1013         assertNull(calibrator.getEstimatedCovariance());
1014         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1015     }
1016 
1017     @Test
1018     public void testConstructor11() throws WrongSizeException {
1019         final Collection<StandardDeviationBodyKinematics> measurements =
1020                 Collections.emptyList();
1021 
1022         final Matrix ba = generateBa();
1023         final double biasX = ba.getElementAtIndex(0);
1024         final double biasY = ba.getElementAtIndex(1);
1025         final double biasZ = ba.getElementAtIndex(2);
1026 
1027         final KnownPositionAccelerometerCalibrator calibrator =
1028                 new KnownPositionAccelerometerCalibrator(measurements,
1029                         biasX, biasY, biasZ);
1030 
1031         // check default values
1032         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1033         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1034         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1035         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1036         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1037         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1038         final Acceleration bx2 = new Acceleration(0.0,
1039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1040         calibrator.getInitialBiasXAsAcceleration(bx2);
1041         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1042         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1043         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1044         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1045         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1046         final Acceleration by2 = new Acceleration(0.0,
1047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1048         calibrator.getInitialBiasYAsAcceleration(by2);
1049         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1050         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1051         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1052         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1053         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1054         final Acceleration bz2 = new Acceleration(0.0,
1055                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1056         calibrator.getInitialBiasZAsAcceleration(bz2);
1057         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1058         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1059         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1060         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1061         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1062         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1063         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1064         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1065         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1066         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1067         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1068         final double[] bias1 = calibrator.getInitialBias();
1069         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1070         final double[] bias2 = new double[3];
1071         calibrator.getInitialBias(bias2);
1072         assertArrayEquals(bias1, bias2, 0.0);
1073         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1074         assertEquals(b1, ba);
1075         final Matrix b2 = new Matrix(3, 1);
1076         calibrator.getInitialBiasAsMatrix(b2);
1077         assertEquals(b1, b2);
1078         final Matrix ma1 = calibrator.getInitialMa();
1079         assertEquals(ma1, new Matrix(3, 3));
1080         final Matrix ma2 = new Matrix(3, 3);
1081         calibrator.getInitialMa(ma2);
1082         assertEquals(ma1, ma2);
1083         assertNull(calibrator.getEcefPosition());
1084         assertNull(calibrator.getNedPosition());
1085         assertFalse(calibrator.getNedPosition(null));
1086         assertSame(calibrator.getMeasurements(), measurements);
1087         assertFalse(calibrator.isCommonAxisUsed());
1088         assertNull(calibrator.getListener());
1089         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1090         assertFalse(calibrator.isReady());
1091         assertFalse(calibrator.isRunning());
1092         assertNull(calibrator.getEstimatedBiases());
1093         assertFalse(calibrator.getEstimatedBiases(null));
1094         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1095         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1096         assertNull(calibrator.getEstimatedBiasFx());
1097         assertNull(calibrator.getEstimatedBiasFy());
1098         assertNull(calibrator.getEstimatedBiasFz());
1099         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1100         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1101         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1102         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1103         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1104         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1105         assertNull(calibrator.getEstimatedMa());
1106         assertNull(calibrator.getEstimatedSx());
1107         assertNull(calibrator.getEstimatedSy());
1108         assertNull(calibrator.getEstimatedSz());
1109         assertNull(calibrator.getEstimatedMxy());
1110         assertNull(calibrator.getEstimatedMxz());
1111         assertNull(calibrator.getEstimatedMyx());
1112         assertNull(calibrator.getEstimatedMyz());
1113         assertNull(calibrator.getEstimatedMzx());
1114         assertNull(calibrator.getEstimatedMzy());
1115         assertNull(calibrator.getEstimatedCovariance());
1116         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1117     }
1118 
1119     @Test
1120     public void testConstructor12() throws WrongSizeException {
1121         final Collection<StandardDeviationBodyKinematics> measurements =
1122                 Collections.emptyList();
1123 
1124         final Matrix ba = generateBa();
1125         final double biasX = ba.getElementAtIndex(0);
1126         final double biasY = ba.getElementAtIndex(1);
1127         final double biasZ = ba.getElementAtIndex(2);
1128 
1129         final KnownPositionAccelerometerCalibrator calibrator =
1130                 new KnownPositionAccelerometerCalibrator(measurements,
1131                         biasX, biasY, biasZ, this);
1132 
1133         // check default values
1134         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1135         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1136         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1137         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1138         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1139         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1140         final Acceleration bx2 = new Acceleration(0.0,
1141                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1142         calibrator.getInitialBiasXAsAcceleration(bx2);
1143         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1144         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1145         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1146         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1147         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1148         final Acceleration by2 = new Acceleration(0.0,
1149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1150         calibrator.getInitialBiasYAsAcceleration(by2);
1151         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1152         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1153         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1154         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1155         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1156         final Acceleration bz2 = new Acceleration(0.0,
1157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1158         calibrator.getInitialBiasZAsAcceleration(bz2);
1159         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1160         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1161         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1162         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1163         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1164         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1165         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1166         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1167         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1168         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1169         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1170         final double[] bias1 = calibrator.getInitialBias();
1171         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1172         final double[] bias2 = new double[3];
1173         calibrator.getInitialBias(bias2);
1174         assertArrayEquals(bias1, bias2, 0.0);
1175         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1176         assertEquals(b1, ba);
1177         final Matrix b2 = new Matrix(3, 1);
1178         calibrator.getInitialBiasAsMatrix(b2);
1179         assertEquals(b1, b2);
1180         final Matrix ma1 = calibrator.getInitialMa();
1181         assertEquals(ma1, new Matrix(3, 3));
1182         final Matrix ma2 = new Matrix(3, 3);
1183         calibrator.getInitialMa(ma2);
1184         assertEquals(ma1, ma2);
1185         assertNull(calibrator.getEcefPosition());
1186         assertNull(calibrator.getNedPosition());
1187         assertFalse(calibrator.getNedPosition(null));
1188         assertSame(calibrator.getMeasurements(), measurements);
1189         assertFalse(calibrator.isCommonAxisUsed());
1190         assertSame(calibrator.getListener(), this);
1191         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1192         assertFalse(calibrator.isReady());
1193         assertFalse(calibrator.isRunning());
1194         assertNull(calibrator.getEstimatedBiases());
1195         assertFalse(calibrator.getEstimatedBiases(null));
1196         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1197         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1198         assertNull(calibrator.getEstimatedBiasFx());
1199         assertNull(calibrator.getEstimatedBiasFy());
1200         assertNull(calibrator.getEstimatedBiasFz());
1201         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1202         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1203         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1204         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1205         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1206         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1207         assertNull(calibrator.getEstimatedMa());
1208         assertNull(calibrator.getEstimatedSx());
1209         assertNull(calibrator.getEstimatedSy());
1210         assertNull(calibrator.getEstimatedSz());
1211         assertNull(calibrator.getEstimatedMxy());
1212         assertNull(calibrator.getEstimatedMxz());
1213         assertNull(calibrator.getEstimatedMyx());
1214         assertNull(calibrator.getEstimatedMyz());
1215         assertNull(calibrator.getEstimatedMzx());
1216         assertNull(calibrator.getEstimatedMzy());
1217         assertNull(calibrator.getEstimatedCovariance());
1218         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1219     }
1220 
1221     @Test
1222     public void testConstructor13() throws WrongSizeException {
1223         final Matrix ba = generateBa();
1224         final double biasX = ba.getElementAtIndex(0);
1225         final double biasY = ba.getElementAtIndex(1);
1226         final double biasZ = ba.getElementAtIndex(2);
1227 
1228         final KnownPositionAccelerometerCalibrator calibrator =
1229                 new KnownPositionAccelerometerCalibrator(true,
1230                         biasX, biasY, biasZ);
1231 
1232         // check default values
1233         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1234         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1235         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1236         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1237         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1238         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1239         final Acceleration bx2 = new Acceleration(0.0,
1240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1241         calibrator.getInitialBiasXAsAcceleration(bx2);
1242         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1243         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1244         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1245         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1246         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1247         final Acceleration by2 = new Acceleration(0.0,
1248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1249         calibrator.getInitialBiasYAsAcceleration(by2);
1250         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1251         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1252         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1253         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1254         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1255         final Acceleration bz2 = new Acceleration(0.0,
1256                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1257         calibrator.getInitialBiasZAsAcceleration(bz2);
1258         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1259         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1260         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1261         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1262         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1263         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1264         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1265         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1266         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1267         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1268         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1269         final double[] bias1 = calibrator.getInitialBias();
1270         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1271         final double[] bias2 = new double[3];
1272         calibrator.getInitialBias(bias2);
1273         assertArrayEquals(bias1, bias2, 0.0);
1274         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1275         assertEquals(b1, ba);
1276         final Matrix b2 = new Matrix(3, 1);
1277         calibrator.getInitialBiasAsMatrix(b2);
1278         assertEquals(b1, b2);
1279         final Matrix ma1 = calibrator.getInitialMa();
1280         assertEquals(ma1, new Matrix(3, 3));
1281         final Matrix ma2 = new Matrix(3, 3);
1282         calibrator.getInitialMa(ma2);
1283         assertEquals(ma1, ma2);
1284         assertNull(calibrator.getEcefPosition());
1285         assertNull(calibrator.getNedPosition());
1286         assertFalse(calibrator.getNedPosition(null));
1287         assertNull(calibrator.getMeasurements());
1288         assertTrue(calibrator.isCommonAxisUsed());
1289         assertNull(calibrator.getListener());
1290         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1291         assertFalse(calibrator.isReady());
1292         assertFalse(calibrator.isRunning());
1293         assertNull(calibrator.getEstimatedBiases());
1294         assertFalse(calibrator.getEstimatedBiases(null));
1295         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1296         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1297         assertNull(calibrator.getEstimatedBiasFx());
1298         assertNull(calibrator.getEstimatedBiasFy());
1299         assertNull(calibrator.getEstimatedBiasFz());
1300         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1301         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1302         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1303         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1304         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1305         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1306         assertNull(calibrator.getEstimatedMa());
1307         assertNull(calibrator.getEstimatedSx());
1308         assertNull(calibrator.getEstimatedSy());
1309         assertNull(calibrator.getEstimatedSz());
1310         assertNull(calibrator.getEstimatedMxy());
1311         assertNull(calibrator.getEstimatedMxz());
1312         assertNull(calibrator.getEstimatedMyx());
1313         assertNull(calibrator.getEstimatedMyz());
1314         assertNull(calibrator.getEstimatedMzx());
1315         assertNull(calibrator.getEstimatedMzy());
1316         assertNull(calibrator.getEstimatedCovariance());
1317         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1318     }
1319 
1320     @Test
1321     public void testConstructor14() throws WrongSizeException {
1322         final Matrix ba = generateBa();
1323         final double biasX = ba.getElementAtIndex(0);
1324         final double biasY = ba.getElementAtIndex(1);
1325         final double biasZ = ba.getElementAtIndex(2);
1326 
1327         final KnownPositionAccelerometerCalibrator calibrator =
1328                 new KnownPositionAccelerometerCalibrator(true,
1329                         biasX, biasY, biasZ, this);
1330 
1331         // check default values
1332         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1333         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1334         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1335         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1336         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1337         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1338         final Acceleration bx2 = new Acceleration(0.0,
1339                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1340         calibrator.getInitialBiasXAsAcceleration(bx2);
1341         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1342         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1343         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1344         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1345         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1346         final Acceleration by2 = new Acceleration(0.0,
1347                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1348         calibrator.getInitialBiasYAsAcceleration(by2);
1349         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1350         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1351         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1352         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1353         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1354         final Acceleration bz2 = new Acceleration(0.0,
1355                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1356         calibrator.getInitialBiasZAsAcceleration(bz2);
1357         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1358         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1359         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1360         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1361         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1362         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1363         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1364         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1365         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1366         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1367         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1368         final double[] bias1 = calibrator.getInitialBias();
1369         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1370         final double[] bias2 = new double[3];
1371         calibrator.getInitialBias(bias2);
1372         assertArrayEquals(bias1, bias2, 0.0);
1373         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1374         assertEquals(b1, ba);
1375         final Matrix b2 = new Matrix(3, 1);
1376         calibrator.getInitialBiasAsMatrix(b2);
1377         assertEquals(b1, b2);
1378         final Matrix ma1 = calibrator.getInitialMa();
1379         assertEquals(ma1, new Matrix(3, 3));
1380         final Matrix ma2 = new Matrix(3, 3);
1381         calibrator.getInitialMa(ma2);
1382         assertEquals(ma1, ma2);
1383         assertNull(calibrator.getEcefPosition());
1384         assertNull(calibrator.getNedPosition());
1385         assertFalse(calibrator.getNedPosition(null));
1386         assertNull(calibrator.getMeasurements());
1387         assertTrue(calibrator.isCommonAxisUsed());
1388         assertSame(calibrator.getListener(), this);
1389         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1390         assertFalse(calibrator.isReady());
1391         assertFalse(calibrator.isRunning());
1392         assertNull(calibrator.getEstimatedBiases());
1393         assertFalse(calibrator.getEstimatedBiases(null));
1394         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1395         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1396         assertNull(calibrator.getEstimatedBiasFx());
1397         assertNull(calibrator.getEstimatedBiasFy());
1398         assertNull(calibrator.getEstimatedBiasFz());
1399         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1400         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1401         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1402         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1403         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1404         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1405         assertNull(calibrator.getEstimatedMa());
1406         assertNull(calibrator.getEstimatedSx());
1407         assertNull(calibrator.getEstimatedSy());
1408         assertNull(calibrator.getEstimatedSz());
1409         assertNull(calibrator.getEstimatedMxy());
1410         assertNull(calibrator.getEstimatedMxz());
1411         assertNull(calibrator.getEstimatedMyx());
1412         assertNull(calibrator.getEstimatedMyz());
1413         assertNull(calibrator.getEstimatedMzx());
1414         assertNull(calibrator.getEstimatedMzy());
1415         assertNull(calibrator.getEstimatedCovariance());
1416         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1417     }
1418 
1419     @Test
1420     public void testConstructor15() throws WrongSizeException {
1421         final Collection<StandardDeviationBodyKinematics> measurements =
1422                 Collections.emptyList();
1423 
1424         final Matrix ba = generateBa();
1425         final double biasX = ba.getElementAtIndex(0);
1426         final double biasY = ba.getElementAtIndex(1);
1427         final double biasZ = ba.getElementAtIndex(2);
1428 
1429         final KnownPositionAccelerometerCalibrator calibrator =
1430                 new KnownPositionAccelerometerCalibrator(measurements,
1431                         true, biasX, biasY, biasZ);
1432 
1433         // check default values
1434         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1435         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1436         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1437         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1438         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1439         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1440         final Acceleration bx2 = new Acceleration(0.0,
1441                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1442         calibrator.getInitialBiasXAsAcceleration(bx2);
1443         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1444         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1446         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1447         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1448         final Acceleration by2 = new Acceleration(0.0,
1449                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1450         calibrator.getInitialBiasYAsAcceleration(by2);
1451         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1452         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1453         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1454         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1455         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1456         final Acceleration bz2 = new Acceleration(0.0,
1457                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1458         calibrator.getInitialBiasZAsAcceleration(bz2);
1459         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1460         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1461         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1462         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1463         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1464         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1465         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1466         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1467         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1468         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1469         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1470         final double[] bias1 = calibrator.getInitialBias();
1471         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1472         final double[] bias2 = new double[3];
1473         calibrator.getInitialBias(bias2);
1474         assertArrayEquals(bias1, bias2, 0.0);
1475         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1476         assertEquals(b1, ba);
1477         final Matrix b2 = new Matrix(3, 1);
1478         calibrator.getInitialBiasAsMatrix(b2);
1479         assertEquals(b1, b2);
1480         final Matrix ma1 = calibrator.getInitialMa();
1481         assertEquals(ma1, new Matrix(3, 3));
1482         final Matrix ma2 = new Matrix(3, 3);
1483         calibrator.getInitialMa(ma2);
1484         assertEquals(ma1, ma2);
1485         assertNull(calibrator.getEcefPosition());
1486         assertNull(calibrator.getNedPosition());
1487         assertFalse(calibrator.getNedPosition(null));
1488         assertSame(calibrator.getMeasurements(), measurements);
1489         assertTrue(calibrator.isCommonAxisUsed());
1490         assertNull(calibrator.getListener());
1491         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1492         assertFalse(calibrator.isReady());
1493         assertFalse(calibrator.isRunning());
1494         assertNull(calibrator.getEstimatedBiases());
1495         assertFalse(calibrator.getEstimatedBiases(null));
1496         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1497         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1498         assertNull(calibrator.getEstimatedBiasFx());
1499         assertNull(calibrator.getEstimatedBiasFy());
1500         assertNull(calibrator.getEstimatedBiasFz());
1501         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1502         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1503         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1504         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1505         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1506         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1507         assertNull(calibrator.getEstimatedMa());
1508         assertNull(calibrator.getEstimatedSx());
1509         assertNull(calibrator.getEstimatedSy());
1510         assertNull(calibrator.getEstimatedSz());
1511         assertNull(calibrator.getEstimatedMxy());
1512         assertNull(calibrator.getEstimatedMxz());
1513         assertNull(calibrator.getEstimatedMyx());
1514         assertNull(calibrator.getEstimatedMyz());
1515         assertNull(calibrator.getEstimatedMzx());
1516         assertNull(calibrator.getEstimatedMzy());
1517         assertNull(calibrator.getEstimatedCovariance());
1518         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1519     }
1520 
1521     @Test
1522     public void testConstructor16() throws WrongSizeException {
1523         final Collection<StandardDeviationBodyKinematics> measurements =
1524                 Collections.emptyList();
1525 
1526         final Matrix ba = generateBa();
1527         final double biasX = ba.getElementAtIndex(0);
1528         final double biasY = ba.getElementAtIndex(1);
1529         final double biasZ = ba.getElementAtIndex(2);
1530 
1531         final KnownPositionAccelerometerCalibrator calibrator =
1532                 new KnownPositionAccelerometerCalibrator(measurements,
1533                         true, biasX, biasY, biasZ, this);
1534 
1535         // check default values
1536         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1537         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1538         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1539         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1540         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1541         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1542         final Acceleration bx2 = new Acceleration(0.0,
1543                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1544         calibrator.getInitialBiasXAsAcceleration(bx2);
1545         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1546         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1547         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1548         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1549         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1550         final Acceleration by2 = new Acceleration(0.0,
1551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1552         calibrator.getInitialBiasYAsAcceleration(by2);
1553         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1554         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1555         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1556         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1557         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1558         final Acceleration bz2 = new Acceleration(0.0,
1559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1560         calibrator.getInitialBiasZAsAcceleration(bz2);
1561         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1562         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1563         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1564         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1565         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1566         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1567         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1568         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1569         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1570         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1571         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1572         final double[] bias1 = calibrator.getInitialBias();
1573         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1574         final double[] bias2 = new double[3];
1575         calibrator.getInitialBias(bias2);
1576         assertArrayEquals(bias1, bias2, 0.0);
1577         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1578         assertEquals(b1, ba);
1579         final Matrix b2 = new Matrix(3, 1);
1580         calibrator.getInitialBiasAsMatrix(b2);
1581         assertEquals(b1, b2);
1582         final Matrix ma1 = calibrator.getInitialMa();
1583         assertEquals(ma1, new Matrix(3, 3));
1584         final Matrix ma2 = new Matrix(3, 3);
1585         calibrator.getInitialMa(ma2);
1586         assertEquals(ma1, ma2);
1587         assertNull(calibrator.getEcefPosition());
1588         assertNull(calibrator.getNedPosition());
1589         assertFalse(calibrator.getNedPosition(null));
1590         assertSame(calibrator.getMeasurements(), measurements);
1591         assertTrue(calibrator.isCommonAxisUsed());
1592         assertSame(calibrator.getListener(), this);
1593         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1594         assertFalse(calibrator.isReady());
1595         assertFalse(calibrator.isRunning());
1596         assertNull(calibrator.getEstimatedBiases());
1597         assertFalse(calibrator.getEstimatedBiases(null));
1598         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1599         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1600         assertNull(calibrator.getEstimatedBiasFx());
1601         assertNull(calibrator.getEstimatedBiasFy());
1602         assertNull(calibrator.getEstimatedBiasFz());
1603         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1604         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1605         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1606         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1607         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1608         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1609         assertNull(calibrator.getEstimatedMa());
1610         assertNull(calibrator.getEstimatedSx());
1611         assertNull(calibrator.getEstimatedSy());
1612         assertNull(calibrator.getEstimatedSz());
1613         assertNull(calibrator.getEstimatedMxy());
1614         assertNull(calibrator.getEstimatedMxz());
1615         assertNull(calibrator.getEstimatedMyx());
1616         assertNull(calibrator.getEstimatedMyz());
1617         assertNull(calibrator.getEstimatedMzx());
1618         assertNull(calibrator.getEstimatedMzy());
1619         assertNull(calibrator.getEstimatedCovariance());
1620         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1621     }
1622 
1623     @Test
1624     public void testConstructor17() throws WrongSizeException {
1625         final Matrix ba = generateBa();
1626         final double biasX = ba.getElementAtIndex(0);
1627         final double biasY = ba.getElementAtIndex(1);
1628         final double biasZ = ba.getElementAtIndex(2);
1629 
1630         final Acceleration bx = new Acceleration(biasX,
1631                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1632         final Acceleration by = new Acceleration(biasY,
1633                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1634         final Acceleration bz = new Acceleration(biasZ,
1635                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1636 
1637         final KnownPositionAccelerometerCalibrator calibrator =
1638                 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1639 
1640         // check default values
1641         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1642         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1643         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1644         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1645         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1646         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1647         final Acceleration bx2 = new Acceleration(0.0,
1648                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1649         calibrator.getInitialBiasXAsAcceleration(bx2);
1650         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1651         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1653         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1654         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1655         final Acceleration by2 = new Acceleration(0.0,
1656                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1657         calibrator.getInitialBiasYAsAcceleration(by2);
1658         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1659         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1661         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1662         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1663         final Acceleration bz2 = new Acceleration(0.0,
1664                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1665         calibrator.getInitialBiasZAsAcceleration(bz2);
1666         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1667         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1669         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1670         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1671         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1672         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1673         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1674         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1675         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1676         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1677         final double[] bias1 = calibrator.getInitialBias();
1678         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1679         final double[] bias2 = new double[3];
1680         calibrator.getInitialBias(bias2);
1681         assertArrayEquals(bias1, bias2, 0.0);
1682         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1683         assertEquals(b1, ba);
1684         final Matrix b2 = new Matrix(3, 1);
1685         calibrator.getInitialBiasAsMatrix(b2);
1686         assertEquals(b1, b2);
1687         final Matrix ma1 = calibrator.getInitialMa();
1688         assertEquals(ma1, new Matrix(3, 3));
1689         final Matrix ma2 = new Matrix(3, 3);
1690         calibrator.getInitialMa(ma2);
1691         assertEquals(ma1, ma2);
1692         assertNull(calibrator.getEcefPosition());
1693         assertNull(calibrator.getNedPosition());
1694         assertFalse(calibrator.getNedPosition(null));
1695         assertNull(calibrator.getMeasurements());
1696         assertFalse(calibrator.isCommonAxisUsed());
1697         assertNull(calibrator.getListener());
1698         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1699         assertFalse(calibrator.isReady());
1700         assertFalse(calibrator.isRunning());
1701         assertNull(calibrator.getEstimatedBiases());
1702         assertFalse(calibrator.getEstimatedBiases(null));
1703         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1704         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1705         assertNull(calibrator.getEstimatedBiasFx());
1706         assertNull(calibrator.getEstimatedBiasFy());
1707         assertNull(calibrator.getEstimatedBiasFz());
1708         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1709         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1710         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1711         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1712         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1713         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1714         assertNull(calibrator.getEstimatedMa());
1715         assertNull(calibrator.getEstimatedSx());
1716         assertNull(calibrator.getEstimatedSy());
1717         assertNull(calibrator.getEstimatedSz());
1718         assertNull(calibrator.getEstimatedMxy());
1719         assertNull(calibrator.getEstimatedMxz());
1720         assertNull(calibrator.getEstimatedMyx());
1721         assertNull(calibrator.getEstimatedMyz());
1722         assertNull(calibrator.getEstimatedMzx());
1723         assertNull(calibrator.getEstimatedMzy());
1724         assertNull(calibrator.getEstimatedCovariance());
1725         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1726     }
1727 
1728     @Test
1729     public void testConstructor18() throws WrongSizeException {
1730         final Matrix ba = generateBa();
1731         final double biasX = ba.getElementAtIndex(0);
1732         final double biasY = ba.getElementAtIndex(1);
1733         final double biasZ = ba.getElementAtIndex(2);
1734 
1735         final Acceleration bx = new Acceleration(biasX,
1736                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1737         final Acceleration by = new Acceleration(biasY,
1738                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1739         final Acceleration bz = new Acceleration(biasZ,
1740                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1741 
1742         final KnownPositionAccelerometerCalibrator calibrator =
1743                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1744                         this);
1745 
1746         // check default values
1747         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1748         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1749         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1750         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1751         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1752         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1753         final Acceleration bx2 = new Acceleration(0.0,
1754                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1755         calibrator.getInitialBiasXAsAcceleration(bx2);
1756         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1757         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1758         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1759         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1760         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1761         final Acceleration by2 = new Acceleration(0.0,
1762                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1763         calibrator.getInitialBiasYAsAcceleration(by2);
1764         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1765         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1767         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1768         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1769         final Acceleration bz2 = new Acceleration(0.0,
1770                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1771         calibrator.getInitialBiasZAsAcceleration(bz2);
1772         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1773         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1774         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1775         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1776         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1777         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1778         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1779         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1780         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1781         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1782         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1783         final double[] bias1 = calibrator.getInitialBias();
1784         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1785         final double[] bias2 = new double[3];
1786         calibrator.getInitialBias(bias2);
1787         assertArrayEquals(bias1, bias2, 0.0);
1788         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1789         assertEquals(b1, ba);
1790         final Matrix b2 = new Matrix(3, 1);
1791         calibrator.getInitialBiasAsMatrix(b2);
1792         assertEquals(b1, b2);
1793         final Matrix ma1 = calibrator.getInitialMa();
1794         assertEquals(ma1, new Matrix(3, 3));
1795         final Matrix ma2 = new Matrix(3, 3);
1796         calibrator.getInitialMa(ma2);
1797         assertEquals(ma1, ma2);
1798         assertNull(calibrator.getEcefPosition());
1799         assertNull(calibrator.getNedPosition());
1800         assertFalse(calibrator.getNedPosition(null));
1801         assertNull(calibrator.getMeasurements());
1802         assertFalse(calibrator.isCommonAxisUsed());
1803         assertSame(calibrator.getListener(), this);
1804         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1805         assertFalse(calibrator.isReady());
1806         assertFalse(calibrator.isRunning());
1807         assertNull(calibrator.getEstimatedBiases());
1808         assertFalse(calibrator.getEstimatedBiases(null));
1809         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1810         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1811         assertNull(calibrator.getEstimatedBiasFx());
1812         assertNull(calibrator.getEstimatedBiasFy());
1813         assertNull(calibrator.getEstimatedBiasFz());
1814         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1815         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1816         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1817         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1818         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1819         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1820         assertNull(calibrator.getEstimatedMa());
1821         assertNull(calibrator.getEstimatedSx());
1822         assertNull(calibrator.getEstimatedSy());
1823         assertNull(calibrator.getEstimatedSz());
1824         assertNull(calibrator.getEstimatedMxy());
1825         assertNull(calibrator.getEstimatedMxz());
1826         assertNull(calibrator.getEstimatedMyx());
1827         assertNull(calibrator.getEstimatedMyz());
1828         assertNull(calibrator.getEstimatedMzx());
1829         assertNull(calibrator.getEstimatedMzy());
1830         assertNull(calibrator.getEstimatedCovariance());
1831         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1832     }
1833 
1834     @Test
1835     public void testConstructor19() throws WrongSizeException {
1836         final Collection<StandardDeviationBodyKinematics> measurements =
1837                 Collections.emptyList();
1838 
1839         final Matrix ba = generateBa();
1840         final double biasX = ba.getElementAtIndex(0);
1841         final double biasY = ba.getElementAtIndex(1);
1842         final double biasZ = ba.getElementAtIndex(2);
1843 
1844         final Acceleration bx = new Acceleration(biasX,
1845                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1846         final Acceleration by = new Acceleration(biasY,
1847                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1848         final Acceleration bz = new Acceleration(biasZ,
1849                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1850 
1851         final KnownPositionAccelerometerCalibrator calibrator =
1852                 new KnownPositionAccelerometerCalibrator(measurements,
1853                         bx, by, bz);
1854 
1855         // check default values
1856         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1857         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1858         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1859         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1860         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1861         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1862         final Acceleration bx2 = new Acceleration(0.0,
1863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1864         calibrator.getInitialBiasXAsAcceleration(bx2);
1865         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1866         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1868         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1869         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1870         final Acceleration by2 = new Acceleration(0.0,
1871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1872         calibrator.getInitialBiasYAsAcceleration(by2);
1873         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1874         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1875         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1876         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1877         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1878         final Acceleration bz2 = new Acceleration(0.0,
1879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1880         calibrator.getInitialBiasZAsAcceleration(bz2);
1881         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1882         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1883         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1884         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1885         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1886         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1887         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1888         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1889         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1890         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1891         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1892         final double[] bias1 = calibrator.getInitialBias();
1893         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
1894         final double[] bias2 = new double[3];
1895         calibrator.getInitialBias(bias2);
1896         assertArrayEquals(bias1, bias2, 0.0);
1897         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1898         assertEquals(b1, ba);
1899         final Matrix b2 = new Matrix(3, 1);
1900         calibrator.getInitialBiasAsMatrix(b2);
1901         assertEquals(b1, b2);
1902         final Matrix ma1 = calibrator.getInitialMa();
1903         assertEquals(ma1, new Matrix(3, 3));
1904         final Matrix ma2 = new Matrix(3, 3);
1905         calibrator.getInitialMa(ma2);
1906         assertEquals(ma1, ma2);
1907         assertNull(calibrator.getEcefPosition());
1908         assertNull(calibrator.getNedPosition());
1909         assertFalse(calibrator.getNedPosition(null));
1910         assertSame(calibrator.getMeasurements(), measurements);
1911         assertFalse(calibrator.isCommonAxisUsed());
1912         assertNull(calibrator.getListener());
1913         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1914         assertFalse(calibrator.isReady());
1915         assertFalse(calibrator.isRunning());
1916         assertNull(calibrator.getEstimatedBiases());
1917         assertFalse(calibrator.getEstimatedBiases(null));
1918         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1919         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1920         assertNull(calibrator.getEstimatedBiasFx());
1921         assertNull(calibrator.getEstimatedBiasFy());
1922         assertNull(calibrator.getEstimatedBiasFz());
1923         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1924         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1925         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1926         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1927         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1928         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1929         assertNull(calibrator.getEstimatedMa());
1930         assertNull(calibrator.getEstimatedSx());
1931         assertNull(calibrator.getEstimatedSy());
1932         assertNull(calibrator.getEstimatedSz());
1933         assertNull(calibrator.getEstimatedMxy());
1934         assertNull(calibrator.getEstimatedMxz());
1935         assertNull(calibrator.getEstimatedMyx());
1936         assertNull(calibrator.getEstimatedMyz());
1937         assertNull(calibrator.getEstimatedMzx());
1938         assertNull(calibrator.getEstimatedMzy());
1939         assertNull(calibrator.getEstimatedCovariance());
1940         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1941     }
1942 
1943     @Test
1944     public void testConstructor20() throws WrongSizeException {
1945         final Collection<StandardDeviationBodyKinematics> measurements =
1946                 Collections.emptyList();
1947 
1948         final Matrix ba = generateBa();
1949         final double biasX = ba.getElementAtIndex(0);
1950         final double biasY = ba.getElementAtIndex(1);
1951         final double biasZ = ba.getElementAtIndex(2);
1952 
1953         final Acceleration bx = new Acceleration(biasX,
1954                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1955         final Acceleration by = new Acceleration(biasY,
1956                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1957         final Acceleration bz = new Acceleration(biasZ,
1958                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1959 
1960         final KnownPositionAccelerometerCalibrator calibrator =
1961                 new KnownPositionAccelerometerCalibrator(measurements,
1962                         bx, by, bz, this);
1963 
1964         // check default values
1965         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1966         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1967         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1968         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1969         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1970         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1971         final Acceleration bx2 = new Acceleration(0.0,
1972                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1973         calibrator.getInitialBiasXAsAcceleration(bx2);
1974         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1975         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1976         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1977         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1978         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1979         final Acceleration by2 = new Acceleration(0.0,
1980                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1981         calibrator.getInitialBiasYAsAcceleration(by2);
1982         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1983         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1984         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1985         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1986         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1987         final Acceleration bz2 = new Acceleration(0.0,
1988                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1989         calibrator.getInitialBiasZAsAcceleration(bz2);
1990         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1991         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1992         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1993         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1994         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1995         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1996         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1997         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1998         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1999         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2000         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2001         final double[] bias1 = calibrator.getInitialBias();
2002         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2003         final double[] bias2 = new double[3];
2004         calibrator.getInitialBias(bias2);
2005         assertArrayEquals(bias1, bias2, 0.0);
2006         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2007         assertEquals(b1, ba);
2008         final Matrix b2 = new Matrix(3, 1);
2009         calibrator.getInitialBiasAsMatrix(b2);
2010         assertEquals(b1, b2);
2011         final Matrix ma1 = calibrator.getInitialMa();
2012         assertEquals(ma1, new Matrix(3, 3));
2013         final Matrix ma2 = new Matrix(3, 3);
2014         calibrator.getInitialMa(ma2);
2015         assertEquals(ma1, ma2);
2016         assertNull(calibrator.getEcefPosition());
2017         assertNull(calibrator.getNedPosition());
2018         assertFalse(calibrator.getNedPosition(null));
2019         assertSame(calibrator.getMeasurements(), measurements);
2020         assertFalse(calibrator.isCommonAxisUsed());
2021         assertSame(calibrator.getListener(), this);
2022         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2023         assertFalse(calibrator.isReady());
2024         assertFalse(calibrator.isRunning());
2025         assertNull(calibrator.getEstimatedBiases());
2026         assertFalse(calibrator.getEstimatedBiases(null));
2027         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2028         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2029         assertNull(calibrator.getEstimatedBiasFx());
2030         assertNull(calibrator.getEstimatedBiasFy());
2031         assertNull(calibrator.getEstimatedBiasFz());
2032         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2033         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2034         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2035         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2036         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2037         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2038         assertNull(calibrator.getEstimatedMa());
2039         assertNull(calibrator.getEstimatedSx());
2040         assertNull(calibrator.getEstimatedSy());
2041         assertNull(calibrator.getEstimatedSz());
2042         assertNull(calibrator.getEstimatedMxy());
2043         assertNull(calibrator.getEstimatedMxz());
2044         assertNull(calibrator.getEstimatedMyx());
2045         assertNull(calibrator.getEstimatedMyz());
2046         assertNull(calibrator.getEstimatedMzx());
2047         assertNull(calibrator.getEstimatedMzy());
2048         assertNull(calibrator.getEstimatedCovariance());
2049         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2050     }
2051 
2052     @Test
2053     public void testConstructor21() throws WrongSizeException {
2054         final Matrix ba = generateBa();
2055         final double biasX = ba.getElementAtIndex(0);
2056         final double biasY = ba.getElementAtIndex(1);
2057         final double biasZ = ba.getElementAtIndex(2);
2058 
2059         final Acceleration bx = new Acceleration(biasX,
2060                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2061         final Acceleration by = new Acceleration(biasY,
2062                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063         final Acceleration bz = new Acceleration(biasZ,
2064                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2065 
2066         final KnownPositionAccelerometerCalibrator calibrator =
2067                 new KnownPositionAccelerometerCalibrator(true,
2068                         bx, by, bz);
2069 
2070         // check default values
2071         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2072         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2073         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2074         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2075         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2076         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2077         final Acceleration bx2 = new Acceleration(0.0,
2078                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2079         calibrator.getInitialBiasXAsAcceleration(bx2);
2080         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2081         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2082         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2083         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2084         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2085         final Acceleration by2 = new Acceleration(0.0,
2086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2087         calibrator.getInitialBiasYAsAcceleration(by2);
2088         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2089         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2090         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2091         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2092         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2093         final Acceleration bz2 = new Acceleration(0.0,
2094                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2095         calibrator.getInitialBiasZAsAcceleration(bz2);
2096         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2097         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2098         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2099         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2100         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2101         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2102         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2103         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2104         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2105         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2106         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2107         final double[] bias1 = calibrator.getInitialBias();
2108         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2109         final double[] bias2 = new double[3];
2110         calibrator.getInitialBias(bias2);
2111         assertArrayEquals(bias1, bias2, 0.0);
2112         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2113         assertEquals(b1, ba);
2114         final Matrix b2 = new Matrix(3, 1);
2115         calibrator.getInitialBiasAsMatrix(b2);
2116         assertEquals(b1, b2);
2117         final Matrix ma1 = calibrator.getInitialMa();
2118         assertEquals(ma1, new Matrix(3, 3));
2119         final Matrix ma2 = new Matrix(3, 3);
2120         calibrator.getInitialMa(ma2);
2121         assertEquals(ma1, ma2);
2122         assertNull(calibrator.getEcefPosition());
2123         assertNull(calibrator.getNedPosition());
2124         assertFalse(calibrator.getNedPosition(null));
2125         assertNull(calibrator.getMeasurements());
2126         assertTrue(calibrator.isCommonAxisUsed());
2127         assertNull(calibrator.getListener());
2128         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2129         assertFalse(calibrator.isReady());
2130         assertFalse(calibrator.isRunning());
2131         assertNull(calibrator.getEstimatedBiases());
2132         assertFalse(calibrator.getEstimatedBiases(null));
2133         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2134         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2135         assertNull(calibrator.getEstimatedBiasFx());
2136         assertNull(calibrator.getEstimatedBiasFy());
2137         assertNull(calibrator.getEstimatedBiasFz());
2138         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2139         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2140         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2141         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2142         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2143         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2144         assertNull(calibrator.getEstimatedMa());
2145         assertNull(calibrator.getEstimatedSx());
2146         assertNull(calibrator.getEstimatedSy());
2147         assertNull(calibrator.getEstimatedSz());
2148         assertNull(calibrator.getEstimatedMxy());
2149         assertNull(calibrator.getEstimatedMxz());
2150         assertNull(calibrator.getEstimatedMyx());
2151         assertNull(calibrator.getEstimatedMyz());
2152         assertNull(calibrator.getEstimatedMzx());
2153         assertNull(calibrator.getEstimatedMzy());
2154         assertNull(calibrator.getEstimatedCovariance());
2155         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2156     }
2157 
2158     @Test
2159     public void testConstructor22() throws WrongSizeException {
2160         final Matrix ba = generateBa();
2161         final double biasX = ba.getElementAtIndex(0);
2162         final double biasY = ba.getElementAtIndex(1);
2163         final double biasZ = ba.getElementAtIndex(2);
2164 
2165         final Acceleration bx = new Acceleration(biasX,
2166                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2167         final Acceleration by = new Acceleration(biasY,
2168                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2169         final Acceleration bz = new Acceleration(biasZ,
2170                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2171 
2172         final KnownPositionAccelerometerCalibrator calibrator =
2173                 new KnownPositionAccelerometerCalibrator(true,
2174                         bx, by, bz, this);
2175 
2176         // check default values
2177         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2178         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2179         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2180         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2181         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2182         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2183         final Acceleration bx2 = new Acceleration(0.0,
2184                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2185         calibrator.getInitialBiasXAsAcceleration(bx2);
2186         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2187         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2188         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2189         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2190         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2191         final Acceleration by2 = new Acceleration(0.0,
2192                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2193         calibrator.getInitialBiasYAsAcceleration(by2);
2194         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2195         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2196         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2197         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2198         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2199         final Acceleration bz2 = new Acceleration(0.0,
2200                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2201         calibrator.getInitialBiasZAsAcceleration(bz2);
2202         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2203         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2204         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2205         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2206         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2207         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2208         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2209         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2210         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2211         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2212         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2213         final double[] bias1 = calibrator.getInitialBias();
2214         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2215         final double[] bias2 = new double[3];
2216         calibrator.getInitialBias(bias2);
2217         assertArrayEquals(bias1, bias2, 0.0);
2218         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2219         assertEquals(b1, ba);
2220         final Matrix b2 = new Matrix(3, 1);
2221         calibrator.getInitialBiasAsMatrix(b2);
2222         assertEquals(b1, b2);
2223         final Matrix ma1 = calibrator.getInitialMa();
2224         assertEquals(ma1, new Matrix(3, 3));
2225         final Matrix ma2 = new Matrix(3, 3);
2226         calibrator.getInitialMa(ma2);
2227         assertEquals(ma1, ma2);
2228         assertNull(calibrator.getEcefPosition());
2229         assertNull(calibrator.getNedPosition());
2230         assertFalse(calibrator.getNedPosition(null));
2231         assertNull(calibrator.getMeasurements());
2232         assertTrue(calibrator.isCommonAxisUsed());
2233         assertSame(calibrator.getListener(), this);
2234         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2235         assertFalse(calibrator.isReady());
2236         assertFalse(calibrator.isRunning());
2237         assertNull(calibrator.getEstimatedBiases());
2238         assertFalse(calibrator.getEstimatedBiases(null));
2239         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2240         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2241         assertNull(calibrator.getEstimatedBiasFx());
2242         assertNull(calibrator.getEstimatedBiasFy());
2243         assertNull(calibrator.getEstimatedBiasFz());
2244         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2245         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2246         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2247         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2248         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2249         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2250         assertNull(calibrator.getEstimatedMa());
2251         assertNull(calibrator.getEstimatedSx());
2252         assertNull(calibrator.getEstimatedSy());
2253         assertNull(calibrator.getEstimatedSz());
2254         assertNull(calibrator.getEstimatedMxy());
2255         assertNull(calibrator.getEstimatedMxz());
2256         assertNull(calibrator.getEstimatedMyx());
2257         assertNull(calibrator.getEstimatedMyz());
2258         assertNull(calibrator.getEstimatedMzx());
2259         assertNull(calibrator.getEstimatedMzy());
2260         assertNull(calibrator.getEstimatedCovariance());
2261         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2262     }
2263 
2264     @Test
2265     public void testConstructor23() throws WrongSizeException {
2266         final Collection<StandardDeviationBodyKinematics> measurements =
2267                 Collections.emptyList();
2268 
2269         final Matrix ba = generateBa();
2270         final double biasX = ba.getElementAtIndex(0);
2271         final double biasY = ba.getElementAtIndex(1);
2272         final double biasZ = ba.getElementAtIndex(2);
2273 
2274         final Acceleration bx = new Acceleration(biasX,
2275                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2276         final Acceleration by = new Acceleration(biasY,
2277                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2278         final Acceleration bz = new Acceleration(biasZ,
2279                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2280 
2281         final KnownPositionAccelerometerCalibrator calibrator =
2282                 new KnownPositionAccelerometerCalibrator(measurements,
2283                         true, bx, by, bz);
2284 
2285         // check default values
2286         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2287         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2288         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2289         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2290         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2291         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2292         final Acceleration bx2 = new Acceleration(0.0,
2293                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2294         calibrator.getInitialBiasXAsAcceleration(bx2);
2295         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2296         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2297         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2298         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2299         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2300         final Acceleration by2 = new Acceleration(0.0,
2301                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2302         calibrator.getInitialBiasYAsAcceleration(by2);
2303         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2304         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2305         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2306         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2307         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2308         final Acceleration bz2 = new Acceleration(0.0,
2309                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2310         calibrator.getInitialBiasZAsAcceleration(bz2);
2311         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2312         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2313         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2314         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2315         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2316         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2317         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2318         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2319         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2320         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2321         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2322         final double[] bias1 = calibrator.getInitialBias();
2323         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2324         final double[] bias2 = new double[3];
2325         calibrator.getInitialBias(bias2);
2326         assertArrayEquals(bias1, bias2, 0.0);
2327         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2328         assertEquals(b1, ba);
2329         final Matrix b2 = new Matrix(3, 1);
2330         calibrator.getInitialBiasAsMatrix(b2);
2331         assertEquals(b1, b2);
2332         final Matrix ma1 = calibrator.getInitialMa();
2333         assertEquals(ma1, new Matrix(3, 3));
2334         final Matrix ma2 = new Matrix(3, 3);
2335         calibrator.getInitialMa(ma2);
2336         assertEquals(ma1, ma2);
2337         assertNull(calibrator.getEcefPosition());
2338         assertNull(calibrator.getNedPosition());
2339         assertFalse(calibrator.getNedPosition(null));
2340         assertSame(calibrator.getMeasurements(), measurements);
2341         assertTrue(calibrator.isCommonAxisUsed());
2342         assertNull(calibrator.getListener());
2343         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2344         assertFalse(calibrator.isReady());
2345         assertFalse(calibrator.isRunning());
2346         assertNull(calibrator.getEstimatedBiases());
2347         assertFalse(calibrator.getEstimatedBiases(null));
2348         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2349         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2350         assertNull(calibrator.getEstimatedBiasFx());
2351         assertNull(calibrator.getEstimatedBiasFy());
2352         assertNull(calibrator.getEstimatedBiasFz());
2353         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2354         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2355         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2356         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2357         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2358         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2359         assertNull(calibrator.getEstimatedMa());
2360         assertNull(calibrator.getEstimatedSx());
2361         assertNull(calibrator.getEstimatedSy());
2362         assertNull(calibrator.getEstimatedSz());
2363         assertNull(calibrator.getEstimatedMxy());
2364         assertNull(calibrator.getEstimatedMxz());
2365         assertNull(calibrator.getEstimatedMyx());
2366         assertNull(calibrator.getEstimatedMyz());
2367         assertNull(calibrator.getEstimatedMzx());
2368         assertNull(calibrator.getEstimatedMzy());
2369         assertNull(calibrator.getEstimatedCovariance());
2370         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2371     }
2372 
2373     @Test
2374     public void testConstructor24() throws WrongSizeException {
2375         final Collection<StandardDeviationBodyKinematics> measurements =
2376                 Collections.emptyList();
2377 
2378         final Matrix ba = generateBa();
2379         final double biasX = ba.getElementAtIndex(0);
2380         final double biasY = ba.getElementAtIndex(1);
2381         final double biasZ = ba.getElementAtIndex(2);
2382 
2383         final Acceleration bx = new Acceleration(biasX,
2384                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2385         final Acceleration by = new Acceleration(biasY,
2386                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2387         final Acceleration bz = new Acceleration(biasZ,
2388                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2389 
2390         final KnownPositionAccelerometerCalibrator calibrator =
2391                 new KnownPositionAccelerometerCalibrator(measurements,
2392                         true, bx, by, bz, this);
2393 
2394         // check default values
2395         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2396         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2397         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2398         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2399         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2400         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2401         final Acceleration bx2 = new Acceleration(0.0,
2402                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2403         calibrator.getInitialBiasXAsAcceleration(bx2);
2404         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2405         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2407         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2408         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2409         final Acceleration by2 = new Acceleration(0.0,
2410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2411         calibrator.getInitialBiasYAsAcceleration(by2);
2412         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2413         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2415         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2416         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2417         final Acceleration bz2 = new Acceleration(0.0,
2418                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2419         calibrator.getInitialBiasZAsAcceleration(bz2);
2420         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2421         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2423         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2424         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2425         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2426         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2427         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2428         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2429         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2430         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2431         final double[] bias1 = calibrator.getInitialBias();
2432         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2433         final double[] bias2 = new double[3];
2434         calibrator.getInitialBias(bias2);
2435         assertArrayEquals(bias1, bias2, 0.0);
2436         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2437         assertEquals(b1, ba);
2438         final Matrix b2 = new Matrix(3, 1);
2439         calibrator.getInitialBiasAsMatrix(b2);
2440         assertEquals(b1, b2);
2441         final Matrix ma1 = calibrator.getInitialMa();
2442         assertEquals(ma1, new Matrix(3, 3));
2443         final Matrix ma2 = new Matrix(3, 3);
2444         calibrator.getInitialMa(ma2);
2445         assertEquals(ma1, ma2);
2446         assertNull(calibrator.getEcefPosition());
2447         assertNull(calibrator.getNedPosition());
2448         assertFalse(calibrator.getNedPosition(null));
2449         assertSame(calibrator.getMeasurements(), measurements);
2450         assertTrue(calibrator.isCommonAxisUsed());
2451         assertSame(calibrator.getListener(), this);
2452         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2453         assertFalse(calibrator.isReady());
2454         assertFalse(calibrator.isRunning());
2455         assertNull(calibrator.getEstimatedBiases());
2456         assertFalse(calibrator.getEstimatedBiases(null));
2457         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2458         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2459         assertNull(calibrator.getEstimatedBiasFx());
2460         assertNull(calibrator.getEstimatedBiasFy());
2461         assertNull(calibrator.getEstimatedBiasFz());
2462         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2463         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2464         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2465         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2466         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2467         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2468         assertNull(calibrator.getEstimatedMa());
2469         assertNull(calibrator.getEstimatedSx());
2470         assertNull(calibrator.getEstimatedSy());
2471         assertNull(calibrator.getEstimatedSz());
2472         assertNull(calibrator.getEstimatedMxy());
2473         assertNull(calibrator.getEstimatedMxz());
2474         assertNull(calibrator.getEstimatedMyx());
2475         assertNull(calibrator.getEstimatedMyz());
2476         assertNull(calibrator.getEstimatedMzx());
2477         assertNull(calibrator.getEstimatedMzy());
2478         assertNull(calibrator.getEstimatedCovariance());
2479         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2480     }
2481 
2482     @Test
2483     public void testConstructor25() throws WrongSizeException {
2484         final Matrix ba = generateBa();
2485         final double biasX = ba.getElementAtIndex(0);
2486         final double biasY = ba.getElementAtIndex(1);
2487         final double biasZ = ba.getElementAtIndex(2);
2488 
2489         final Matrix ma = generateMaCommonAxis();
2490         final double sx = ma.getElementAt(0, 0);
2491         final double sy = ma.getElementAt(1, 1);
2492         final double sz = ma.getElementAt(2, 2);
2493 
2494         final KnownPositionAccelerometerCalibrator calibrator =
2495                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2496                         sx, sy, sz);
2497 
2498         // check default values
2499         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2500         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2501         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2502         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2503         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2504         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505         final Acceleration bx2 = new Acceleration(0.0,
2506                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2507         calibrator.getInitialBiasXAsAcceleration(bx2);
2508         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2509         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2510         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2511         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2512         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2513         final Acceleration by2 = new Acceleration(0.0,
2514                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2515         calibrator.getInitialBiasYAsAcceleration(by2);
2516         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2517         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2518         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2519         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2520         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2521         final Acceleration bz2 = new Acceleration(0.0,
2522                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2523         calibrator.getInitialBiasZAsAcceleration(bz2);
2524         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2525         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2526         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2527         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2528         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2529         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2530         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2531         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2532         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2533         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2534         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2535         final double[] bias1 = calibrator.getInitialBias();
2536         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2537         final double[] bias2 = new double[3];
2538         calibrator.getInitialBias(bias2);
2539         assertArrayEquals(bias1, bias2, 0.0);
2540         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2541         assertEquals(b1, ba);
2542         final Matrix b2 = new Matrix(3, 1);
2543         calibrator.getInitialBiasAsMatrix(b2);
2544         assertEquals(b1, b2);
2545         final Matrix ma1 = calibrator.getInitialMa();
2546         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2547         final Matrix ma2 = new Matrix(3, 3);
2548         calibrator.getInitialMa(ma2);
2549         assertEquals(ma1, ma2);
2550         assertNull(calibrator.getEcefPosition());
2551         assertNull(calibrator.getNedPosition());
2552         assertFalse(calibrator.getNedPosition(null));
2553         assertNull(calibrator.getMeasurements());
2554         assertFalse(calibrator.isCommonAxisUsed());
2555         assertNull(calibrator.getListener());
2556         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2557         assertFalse(calibrator.isReady());
2558         assertFalse(calibrator.isRunning());
2559         assertNull(calibrator.getEstimatedBiases());
2560         assertFalse(calibrator.getEstimatedBiases(null));
2561         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2562         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2563         assertNull(calibrator.getEstimatedBiasFx());
2564         assertNull(calibrator.getEstimatedBiasFy());
2565         assertNull(calibrator.getEstimatedBiasFz());
2566         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2567         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2568         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2569         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2570         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2571         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2572         assertNull(calibrator.getEstimatedMa());
2573         assertNull(calibrator.getEstimatedSx());
2574         assertNull(calibrator.getEstimatedSy());
2575         assertNull(calibrator.getEstimatedSz());
2576         assertNull(calibrator.getEstimatedMxy());
2577         assertNull(calibrator.getEstimatedMxz());
2578         assertNull(calibrator.getEstimatedMyx());
2579         assertNull(calibrator.getEstimatedMyz());
2580         assertNull(calibrator.getEstimatedMzx());
2581         assertNull(calibrator.getEstimatedMzy());
2582         assertNull(calibrator.getEstimatedCovariance());
2583         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2584     }
2585 
2586     @Test
2587     public void testConstructor26() throws WrongSizeException {
2588         final Collection<StandardDeviationBodyKinematics> measurements =
2589                 Collections.emptyList();
2590 
2591         final Matrix ba = generateBa();
2592         final double biasX = ba.getElementAtIndex(0);
2593         final double biasY = ba.getElementAtIndex(1);
2594         final double biasZ = ba.getElementAtIndex(2);
2595 
2596         final Matrix ma = generateMaCommonAxis();
2597         final double sx = ma.getElementAt(0, 0);
2598         final double sy = ma.getElementAt(1, 1);
2599         final double sz = ma.getElementAt(2, 2);
2600 
2601         final KnownPositionAccelerometerCalibrator calibrator =
2602                 new KnownPositionAccelerometerCalibrator(measurements,
2603                         biasX, biasY, biasZ, sx, sy, sz);
2604 
2605         // check default values
2606         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2607         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2608         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2609         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2610         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2611         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2612         final Acceleration bx2 = new Acceleration(0.0,
2613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2614         calibrator.getInitialBiasXAsAcceleration(bx2);
2615         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2616         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2617         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2618         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2619         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2620         final Acceleration by2 = new Acceleration(0.0,
2621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2622         calibrator.getInitialBiasYAsAcceleration(by2);
2623         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2624         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2625         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2626         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2627         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2628         final Acceleration bz2 = new Acceleration(0.0,
2629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2630         calibrator.getInitialBiasZAsAcceleration(bz2);
2631         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2632         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2633         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2634         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2635         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2636         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2637         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2638         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2639         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2640         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2641         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2642         final double[] bias1 = calibrator.getInitialBias();
2643         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2644         final double[] bias2 = new double[3];
2645         calibrator.getInitialBias(bias2);
2646         assertArrayEquals(bias1, bias2, 0.0);
2647         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2648         assertEquals(b1, ba);
2649         final Matrix b2 = new Matrix(3, 1);
2650         calibrator.getInitialBiasAsMatrix(b2);
2651         assertEquals(b1, b2);
2652         final Matrix ma1 = calibrator.getInitialMa();
2653         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2654         final Matrix ma2 = new Matrix(3, 3);
2655         calibrator.getInitialMa(ma2);
2656         assertEquals(ma1, ma2);
2657         assertNull(calibrator.getEcefPosition());
2658         assertNull(calibrator.getNedPosition());
2659         assertFalse(calibrator.getNedPosition(null));
2660         assertSame(calibrator.getMeasurements(), measurements);
2661         assertFalse(calibrator.isCommonAxisUsed());
2662         assertNull(calibrator.getListener());
2663         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2664         assertFalse(calibrator.isReady());
2665         assertFalse(calibrator.isRunning());
2666         assertNull(calibrator.getEstimatedBiases());
2667         assertFalse(calibrator.getEstimatedBiases(null));
2668         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2669         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2670         assertNull(calibrator.getEstimatedBiasFx());
2671         assertNull(calibrator.getEstimatedBiasFy());
2672         assertNull(calibrator.getEstimatedBiasFz());
2673         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2674         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2675         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2676         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2677         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2678         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2679         assertNull(calibrator.getEstimatedMa());
2680         assertNull(calibrator.getEstimatedSx());
2681         assertNull(calibrator.getEstimatedSy());
2682         assertNull(calibrator.getEstimatedSz());
2683         assertNull(calibrator.getEstimatedMxy());
2684         assertNull(calibrator.getEstimatedMxz());
2685         assertNull(calibrator.getEstimatedMyx());
2686         assertNull(calibrator.getEstimatedMyz());
2687         assertNull(calibrator.getEstimatedMzx());
2688         assertNull(calibrator.getEstimatedMzy());
2689         assertNull(calibrator.getEstimatedCovariance());
2690         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2691     }
2692 
2693     @Test
2694     public void testConstructor27() throws WrongSizeException {
2695         final Collection<StandardDeviationBodyKinematics> measurements =
2696                 Collections.emptyList();
2697 
2698         final Matrix ba = generateBa();
2699         final double biasX = ba.getElementAtIndex(0);
2700         final double biasY = ba.getElementAtIndex(1);
2701         final double biasZ = ba.getElementAtIndex(2);
2702 
2703         final Matrix ma = generateMaCommonAxis();
2704         final double sx = ma.getElementAt(0, 0);
2705         final double sy = ma.getElementAt(1, 1);
2706         final double sz = ma.getElementAt(2, 2);
2707 
2708         final KnownPositionAccelerometerCalibrator calibrator =
2709                 new KnownPositionAccelerometerCalibrator(measurements,
2710                         biasX, biasY, biasZ, sx, sy, sz, this);
2711 
2712         // check default values
2713         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2714         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2715         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2716         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2717         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2718         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2719         final Acceleration bx2 = new Acceleration(0.0,
2720                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2721         calibrator.getInitialBiasXAsAcceleration(bx2);
2722         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2723         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2724         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2725         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2726         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2727         final Acceleration by2 = new Acceleration(0.0,
2728                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2729         calibrator.getInitialBiasYAsAcceleration(by2);
2730         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2731         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2732         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2733         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2734         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2735         final Acceleration bz2 = new Acceleration(0.0,
2736                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2737         calibrator.getInitialBiasZAsAcceleration(bz2);
2738         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2739         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2740         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2741         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2742         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2743         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2744         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2745         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2746         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2747         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2748         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2749         final double[] bias1 = calibrator.getInitialBias();
2750         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2751         final double[] bias2 = new double[3];
2752         calibrator.getInitialBias(bias2);
2753         assertArrayEquals(bias1, bias2, 0.0);
2754         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2755         assertEquals(b1, ba);
2756         final Matrix b2 = new Matrix(3, 1);
2757         calibrator.getInitialBiasAsMatrix(b2);
2758         assertEquals(b1, b2);
2759         final Matrix ma1 = calibrator.getInitialMa();
2760         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2761         final Matrix ma2 = new Matrix(3, 3);
2762         calibrator.getInitialMa(ma2);
2763         assertEquals(ma1, ma2);
2764         assertNull(calibrator.getEcefPosition());
2765         assertNull(calibrator.getNedPosition());
2766         assertFalse(calibrator.getNedPosition(null));
2767         assertSame(calibrator.getMeasurements(), measurements);
2768         assertFalse(calibrator.isCommonAxisUsed());
2769         assertSame(calibrator.getListener(), this);
2770         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2771         assertFalse(calibrator.isReady());
2772         assertFalse(calibrator.isRunning());
2773         assertNull(calibrator.getEstimatedBiases());
2774         assertFalse(calibrator.getEstimatedBiases(null));
2775         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2776         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2777         assertNull(calibrator.getEstimatedBiasFx());
2778         assertNull(calibrator.getEstimatedBiasFy());
2779         assertNull(calibrator.getEstimatedBiasFz());
2780         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2781         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2782         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2783         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2784         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2785         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2786         assertNull(calibrator.getEstimatedMa());
2787         assertNull(calibrator.getEstimatedSx());
2788         assertNull(calibrator.getEstimatedSy());
2789         assertNull(calibrator.getEstimatedSz());
2790         assertNull(calibrator.getEstimatedMxy());
2791         assertNull(calibrator.getEstimatedMxz());
2792         assertNull(calibrator.getEstimatedMyx());
2793         assertNull(calibrator.getEstimatedMyz());
2794         assertNull(calibrator.getEstimatedMzx());
2795         assertNull(calibrator.getEstimatedMzy());
2796         assertNull(calibrator.getEstimatedCovariance());
2797         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2798     }
2799 
2800     @Test
2801     public void testConstructor28() throws WrongSizeException {
2802         final Matrix ba = generateBa();
2803         final double biasX = ba.getElementAtIndex(0);
2804         final double biasY = ba.getElementAtIndex(1);
2805         final double biasZ = ba.getElementAtIndex(2);
2806 
2807         final Matrix ma = generateMaCommonAxis();
2808         final double sx = ma.getElementAt(0, 0);
2809         final double sy = ma.getElementAt(1, 1);
2810         final double sz = ma.getElementAt(2, 2);
2811 
2812         final KnownPositionAccelerometerCalibrator calibrator =
2813                 new KnownPositionAccelerometerCalibrator(true,
2814                         biasX, biasY, biasZ, sx, sy, sz);
2815 
2816         // check default values
2817         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2818         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2819         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2820         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2821         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2822         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2823         final Acceleration bx2 = new Acceleration(0.0,
2824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2825         calibrator.getInitialBiasXAsAcceleration(bx2);
2826         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2827         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2828         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2829         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2830         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2831         final Acceleration by2 = new Acceleration(0.0,
2832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2833         calibrator.getInitialBiasYAsAcceleration(by2);
2834         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2835         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2836         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2837         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2838         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2839         final Acceleration bz2 = new Acceleration(0.0,
2840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2841         calibrator.getInitialBiasZAsAcceleration(bz2);
2842         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2843         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2844         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2845         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2846         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2847         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2848         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2849         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2850         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2851         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2852         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2853         final double[] bias1 = calibrator.getInitialBias();
2854         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2855         final double[] bias2 = new double[3];
2856         calibrator.getInitialBias(bias2);
2857         assertArrayEquals(bias1, bias2, 0.0);
2858         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2859         assertEquals(b1, ba);
2860         final Matrix b2 = new Matrix(3, 1);
2861         calibrator.getInitialBiasAsMatrix(b2);
2862         assertEquals(b1, b2);
2863         final Matrix ma1 = calibrator.getInitialMa();
2864         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2865         final Matrix ma2 = new Matrix(3, 3);
2866         calibrator.getInitialMa(ma2);
2867         assertEquals(ma1, ma2);
2868         assertNull(calibrator.getEcefPosition());
2869         assertNull(calibrator.getNedPosition());
2870         assertFalse(calibrator.getNedPosition(null));
2871         assertNull(calibrator.getMeasurements());
2872         assertTrue(calibrator.isCommonAxisUsed());
2873         assertNull(calibrator.getListener());
2874         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2875         assertFalse(calibrator.isReady());
2876         assertFalse(calibrator.isRunning());
2877         assertNull(calibrator.getEstimatedBiases());
2878         assertFalse(calibrator.getEstimatedBiases(null));
2879         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2880         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2881         assertNull(calibrator.getEstimatedBiasFx());
2882         assertNull(calibrator.getEstimatedBiasFy());
2883         assertNull(calibrator.getEstimatedBiasFz());
2884         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2885         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2886         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2887         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2888         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2889         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2890         assertNull(calibrator.getEstimatedMa());
2891         assertNull(calibrator.getEstimatedSx());
2892         assertNull(calibrator.getEstimatedSy());
2893         assertNull(calibrator.getEstimatedSz());
2894         assertNull(calibrator.getEstimatedMxy());
2895         assertNull(calibrator.getEstimatedMxz());
2896         assertNull(calibrator.getEstimatedMyx());
2897         assertNull(calibrator.getEstimatedMyz());
2898         assertNull(calibrator.getEstimatedMzx());
2899         assertNull(calibrator.getEstimatedMzy());
2900         assertNull(calibrator.getEstimatedCovariance());
2901         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2902     }
2903 
2904     @Test
2905     public void testConstructor29() throws WrongSizeException {
2906         final Matrix ba = generateBa();
2907         final double biasX = ba.getElementAtIndex(0);
2908         final double biasY = ba.getElementAtIndex(1);
2909         final double biasZ = ba.getElementAtIndex(2);
2910 
2911         final Matrix ma = generateMaCommonAxis();
2912         final double sx = ma.getElementAt(0, 0);
2913         final double sy = ma.getElementAt(1, 1);
2914         final double sz = ma.getElementAt(2, 2);
2915 
2916         final KnownPositionAccelerometerCalibrator calibrator =
2917                 new KnownPositionAccelerometerCalibrator(true,
2918                         biasX, biasY, biasZ, sx, sy, sz, this);
2919 
2920         // check default values
2921         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2922         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2923         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2924         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2925         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2926         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2927         final Acceleration bx2 = new Acceleration(0.0,
2928                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2929         calibrator.getInitialBiasXAsAcceleration(bx2);
2930         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2931         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2932         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2933         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2934         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2935         final Acceleration by2 = new Acceleration(0.0,
2936                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2937         calibrator.getInitialBiasYAsAcceleration(by2);
2938         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2939         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2940         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2941         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2942         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2943         final Acceleration bz2 = new Acceleration(0.0,
2944                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2945         calibrator.getInitialBiasZAsAcceleration(bz2);
2946         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2947         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2948         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2949         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2950         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2951         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2952         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2953         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2954         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2955         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2956         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2957         final double[] bias1 = calibrator.getInitialBias();
2958         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
2959         final double[] bias2 = new double[3];
2960         calibrator.getInitialBias(bias2);
2961         assertArrayEquals(bias1, bias2, 0.0);
2962         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2963         assertEquals(b1, ba);
2964         final Matrix b2 = new Matrix(3, 1);
2965         calibrator.getInitialBiasAsMatrix(b2);
2966         assertEquals(b1, b2);
2967         final Matrix ma1 = calibrator.getInitialMa();
2968         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
2969         final Matrix ma2 = new Matrix(3, 3);
2970         calibrator.getInitialMa(ma2);
2971         assertEquals(ma1, ma2);
2972         assertNull(calibrator.getEcefPosition());
2973         assertNull(calibrator.getNedPosition());
2974         assertFalse(calibrator.getNedPosition(null));
2975         assertNull(calibrator.getMeasurements());
2976         assertTrue(calibrator.isCommonAxisUsed());
2977         assertSame(calibrator.getListener(), this);
2978         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2979         assertFalse(calibrator.isReady());
2980         assertFalse(calibrator.isRunning());
2981         assertNull(calibrator.getEstimatedBiases());
2982         assertFalse(calibrator.getEstimatedBiases(null));
2983         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2984         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2985         assertNull(calibrator.getEstimatedBiasFx());
2986         assertNull(calibrator.getEstimatedBiasFy());
2987         assertNull(calibrator.getEstimatedBiasFz());
2988         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2989         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2990         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2991         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2992         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2993         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2994         assertNull(calibrator.getEstimatedMa());
2995         assertNull(calibrator.getEstimatedSx());
2996         assertNull(calibrator.getEstimatedSy());
2997         assertNull(calibrator.getEstimatedSz());
2998         assertNull(calibrator.getEstimatedMxy());
2999         assertNull(calibrator.getEstimatedMxz());
3000         assertNull(calibrator.getEstimatedMyx());
3001         assertNull(calibrator.getEstimatedMyz());
3002         assertNull(calibrator.getEstimatedMzx());
3003         assertNull(calibrator.getEstimatedMzy());
3004         assertNull(calibrator.getEstimatedCovariance());
3005         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3006     }
3007 
3008     @Test
3009     public void testConstructor30() throws WrongSizeException {
3010         final Collection<StandardDeviationBodyKinematics> measurements =
3011                 Collections.emptyList();
3012         final Matrix ba = generateBa();
3013         final double biasX = ba.getElementAtIndex(0);
3014         final double biasY = ba.getElementAtIndex(1);
3015         final double biasZ = ba.getElementAtIndex(2);
3016 
3017         final Matrix ma = generateMaCommonAxis();
3018         final double sx = ma.getElementAt(0, 0);
3019         final double sy = ma.getElementAt(1, 1);
3020         final double sz = ma.getElementAt(2, 2);
3021 
3022         final KnownPositionAccelerometerCalibrator calibrator =
3023                 new KnownPositionAccelerometerCalibrator(measurements,
3024                         true, biasX, biasY, biasZ, sx, sy, sz);
3025 
3026         // check default values
3027         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3028         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3029         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3030         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3031         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3032         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3033         final Acceleration bx2 = new Acceleration(0.0,
3034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3035         calibrator.getInitialBiasXAsAcceleration(bx2);
3036         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3037         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3038         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3039         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3040         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3041         final Acceleration by2 = new Acceleration(0.0,
3042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3043         calibrator.getInitialBiasYAsAcceleration(by2);
3044         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3045         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3047         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3048         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3049         final Acceleration bz2 = new Acceleration(0.0,
3050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3051         calibrator.getInitialBiasZAsAcceleration(bz2);
3052         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3053         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3054         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3055         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3056         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3057         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3058         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3059         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3060         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3061         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3062         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3063         final double[] bias1 = calibrator.getInitialBias();
3064         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3065         final double[] bias2 = new double[3];
3066         calibrator.getInitialBias(bias2);
3067         assertArrayEquals(bias1, bias2, 0.0);
3068         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3069         assertEquals(b1, ba);
3070         final Matrix b2 = new Matrix(3, 1);
3071         calibrator.getInitialBiasAsMatrix(b2);
3072         assertEquals(b1, b2);
3073         final Matrix ma1 = calibrator.getInitialMa();
3074         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3075         final Matrix ma2 = new Matrix(3, 3);
3076         calibrator.getInitialMa(ma2);
3077         assertEquals(ma1, ma2);
3078         assertNull(calibrator.getEcefPosition());
3079         assertNull(calibrator.getNedPosition());
3080         assertFalse(calibrator.getNedPosition(null));
3081         assertSame(calibrator.getMeasurements(), measurements);
3082         assertTrue(calibrator.isCommonAxisUsed());
3083         assertNull(calibrator.getListener());
3084         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3085         assertFalse(calibrator.isReady());
3086         assertFalse(calibrator.isRunning());
3087         assertNull(calibrator.getEstimatedBiases());
3088         assertFalse(calibrator.getEstimatedBiases(null));
3089         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3090         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3091         assertNull(calibrator.getEstimatedBiasFx());
3092         assertNull(calibrator.getEstimatedBiasFy());
3093         assertNull(calibrator.getEstimatedBiasFz());
3094         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3095         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3096         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3097         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3098         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3099         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3100         assertNull(calibrator.getEstimatedMa());
3101         assertNull(calibrator.getEstimatedSx());
3102         assertNull(calibrator.getEstimatedSy());
3103         assertNull(calibrator.getEstimatedSz());
3104         assertNull(calibrator.getEstimatedMxy());
3105         assertNull(calibrator.getEstimatedMxz());
3106         assertNull(calibrator.getEstimatedMyx());
3107         assertNull(calibrator.getEstimatedMyz());
3108         assertNull(calibrator.getEstimatedMzx());
3109         assertNull(calibrator.getEstimatedMzy());
3110         assertNull(calibrator.getEstimatedCovariance());
3111         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3112     }
3113 
3114     @Test
3115     public void testConstructor31() throws WrongSizeException {
3116         final Collection<StandardDeviationBodyKinematics> measurements =
3117                 Collections.emptyList();
3118         final Matrix ba = generateBa();
3119         final double biasX = ba.getElementAtIndex(0);
3120         final double biasY = ba.getElementAtIndex(1);
3121         final double biasZ = ba.getElementAtIndex(2);
3122 
3123         final Matrix ma = generateMaCommonAxis();
3124         final double sx = ma.getElementAt(0, 0);
3125         final double sy = ma.getElementAt(1, 1);
3126         final double sz = ma.getElementAt(2, 2);
3127 
3128         final KnownPositionAccelerometerCalibrator calibrator =
3129                 new KnownPositionAccelerometerCalibrator(measurements,
3130                         true, biasX, biasY, biasZ, sx, sy, sz,
3131                         this);
3132 
3133         // check default values
3134         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3135         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3136         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3137         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3138         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3139         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3140         final Acceleration bx2 = new Acceleration(0.0,
3141                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3142         calibrator.getInitialBiasXAsAcceleration(bx2);
3143         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3144         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3145         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3146         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3147         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3148         final Acceleration by2 = new Acceleration(0.0,
3149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3150         calibrator.getInitialBiasYAsAcceleration(by2);
3151         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3152         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3153         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3154         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3155         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156         final Acceleration bz2 = new Acceleration(0.0,
3157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3158         calibrator.getInitialBiasZAsAcceleration(bz2);
3159         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3160         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3161         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3162         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3163         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3164         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3165         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3166         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3167         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3168         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3169         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3170         final double[] bias1 = calibrator.getInitialBias();
3171         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3172         final double[] bias2 = new double[3];
3173         calibrator.getInitialBias(bias2);
3174         assertArrayEquals(bias1, bias2, 0.0);
3175         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3176         assertEquals(b1, ba);
3177         final Matrix b2 = new Matrix(3, 1);
3178         calibrator.getInitialBiasAsMatrix(b2);
3179         assertEquals(b1, b2);
3180         final Matrix ma1 = calibrator.getInitialMa();
3181         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3182         final Matrix ma2 = new Matrix(3, 3);
3183         calibrator.getInitialMa(ma2);
3184         assertEquals(ma1, ma2);
3185         assertNull(calibrator.getEcefPosition());
3186         assertNull(calibrator.getNedPosition());
3187         assertFalse(calibrator.getNedPosition(null));
3188         assertSame(calibrator.getMeasurements(), measurements);
3189         assertTrue(calibrator.isCommonAxisUsed());
3190         assertSame(calibrator.getListener(), this);
3191         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3192         assertFalse(calibrator.isReady());
3193         assertFalse(calibrator.isRunning());
3194         assertNull(calibrator.getEstimatedBiases());
3195         assertFalse(calibrator.getEstimatedBiases(null));
3196         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3197         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3198         assertNull(calibrator.getEstimatedBiasFx());
3199         assertNull(calibrator.getEstimatedBiasFy());
3200         assertNull(calibrator.getEstimatedBiasFz());
3201         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3202         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3203         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3204         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3205         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3206         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3207         assertNull(calibrator.getEstimatedMa());
3208         assertNull(calibrator.getEstimatedSx());
3209         assertNull(calibrator.getEstimatedSy());
3210         assertNull(calibrator.getEstimatedSz());
3211         assertNull(calibrator.getEstimatedMxy());
3212         assertNull(calibrator.getEstimatedMxz());
3213         assertNull(calibrator.getEstimatedMyx());
3214         assertNull(calibrator.getEstimatedMyz());
3215         assertNull(calibrator.getEstimatedMzx());
3216         assertNull(calibrator.getEstimatedMzy());
3217         assertNull(calibrator.getEstimatedCovariance());
3218         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3219     }
3220 
3221     @Test
3222     public void testConstructor32() throws WrongSizeException {
3223         final Matrix ba = generateBa();
3224         final double biasX = ba.getElementAtIndex(0);
3225         final double biasY = ba.getElementAtIndex(1);
3226         final double biasZ = ba.getElementAtIndex(2);
3227 
3228         final Matrix ma = generateMaCommonAxis();
3229         final double sx = ma.getElementAt(0, 0);
3230         final double sy = ma.getElementAt(1, 1);
3231         final double sz = ma.getElementAt(2, 2);
3232 
3233         final Acceleration bx = new Acceleration(biasX,
3234                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3235         final Acceleration by = new Acceleration(biasY,
3236                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3237         final Acceleration bz = new Acceleration(biasZ,
3238                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3239 
3240         final KnownPositionAccelerometerCalibrator calibrator =
3241                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3242                         sx, sy, sz);
3243 
3244         // check default values
3245         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3246         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3247         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3248         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3249         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3250         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3251         final Acceleration bx2 = new Acceleration(0.0,
3252                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3253         calibrator.getInitialBiasXAsAcceleration(bx2);
3254         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3255         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3256         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3257         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3258         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3259         final Acceleration by2 = new Acceleration(0.0,
3260                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3261         calibrator.getInitialBiasYAsAcceleration(by2);
3262         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3263         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3264         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3265         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3266         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3267         final Acceleration bz2 = new Acceleration(0.0,
3268                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3269         calibrator.getInitialBiasZAsAcceleration(bz2);
3270         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3271         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3272         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3273         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3274         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3275         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3276         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3277         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3278         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3279         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3280         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3281         final double[] bias1 = calibrator.getInitialBias();
3282         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3283         final double[] bias2 = new double[3];
3284         calibrator.getInitialBias(bias2);
3285         assertArrayEquals(bias1, bias2, 0.0);
3286         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3287         assertEquals(b1, ba);
3288         final Matrix b2 = new Matrix(3, 1);
3289         calibrator.getInitialBiasAsMatrix(b2);
3290         assertEquals(b1, b2);
3291         final Matrix ma1 = calibrator.getInitialMa();
3292         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3293         final Matrix ma2 = new Matrix(3, 3);
3294         calibrator.getInitialMa(ma2);
3295         assertEquals(ma1, ma2);
3296         assertNull(calibrator.getEcefPosition());
3297         assertNull(calibrator.getNedPosition());
3298         assertFalse(calibrator.getNedPosition(null));
3299         assertNull(calibrator.getMeasurements());
3300         assertFalse(calibrator.isCommonAxisUsed());
3301         assertNull(calibrator.getListener());
3302         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3303         assertFalse(calibrator.isReady());
3304         assertFalse(calibrator.isRunning());
3305         assertNull(calibrator.getEstimatedBiases());
3306         assertFalse(calibrator.getEstimatedBiases(null));
3307         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3308         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3309         assertNull(calibrator.getEstimatedBiasFx());
3310         assertNull(calibrator.getEstimatedBiasFy());
3311         assertNull(calibrator.getEstimatedBiasFz());
3312         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3313         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3314         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3315         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3316         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3317         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3318         assertNull(calibrator.getEstimatedMa());
3319         assertNull(calibrator.getEstimatedSx());
3320         assertNull(calibrator.getEstimatedSy());
3321         assertNull(calibrator.getEstimatedSz());
3322         assertNull(calibrator.getEstimatedMxy());
3323         assertNull(calibrator.getEstimatedMxz());
3324         assertNull(calibrator.getEstimatedMyx());
3325         assertNull(calibrator.getEstimatedMyz());
3326         assertNull(calibrator.getEstimatedMzx());
3327         assertNull(calibrator.getEstimatedMzy());
3328         assertNull(calibrator.getEstimatedCovariance());
3329         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3330     }
3331 
3332     @Test
3333     public void testConstructor33() throws WrongSizeException {
3334         final Matrix ba = generateBa();
3335         final double biasX = ba.getElementAtIndex(0);
3336         final double biasY = ba.getElementAtIndex(1);
3337         final double biasZ = ba.getElementAtIndex(2);
3338 
3339         final Matrix ma = generateMaCommonAxis();
3340         final double sx = ma.getElementAt(0, 0);
3341         final double sy = ma.getElementAt(1, 1);
3342         final double sz = ma.getElementAt(2, 2);
3343 
3344         final Acceleration bx = new Acceleration(biasX,
3345                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3346         final Acceleration by = new Acceleration(biasY,
3347                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3348         final Acceleration bz = new Acceleration(biasZ,
3349                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3350 
3351         final KnownPositionAccelerometerCalibrator calibrator =
3352                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3353                         sx, sy, sz, this);
3354 
3355         // check default values
3356         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3357         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3358         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3359         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3360         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3361         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3362         final Acceleration bx2 = new Acceleration(0.0,
3363                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3364         calibrator.getInitialBiasXAsAcceleration(bx2);
3365         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3366         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3367         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3368         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3369         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3370         final Acceleration by2 = new Acceleration(0.0,
3371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3372         calibrator.getInitialBiasYAsAcceleration(by2);
3373         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3374         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3375         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3376         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3377         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3378         final Acceleration bz2 = new Acceleration(0.0,
3379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3380         calibrator.getInitialBiasZAsAcceleration(bz2);
3381         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3382         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3383         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3384         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3385         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3386         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3387         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3388         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3389         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3390         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3391         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3392         final double[] bias1 = calibrator.getInitialBias();
3393         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3394         final double[] bias2 = new double[3];
3395         calibrator.getInitialBias(bias2);
3396         assertArrayEquals(bias1, bias2, 0.0);
3397         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3398         assertEquals(b1, ba);
3399         final Matrix b2 = new Matrix(3, 1);
3400         calibrator.getInitialBiasAsMatrix(b2);
3401         assertEquals(b1, b2);
3402         final Matrix ma1 = calibrator.getInitialMa();
3403         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3404         final Matrix ma2 = new Matrix(3, 3);
3405         calibrator.getInitialMa(ma2);
3406         assertEquals(ma1, ma2);
3407         assertNull(calibrator.getEcefPosition());
3408         assertNull(calibrator.getNedPosition());
3409         assertFalse(calibrator.getNedPosition(null));
3410         assertNull(calibrator.getMeasurements());
3411         assertFalse(calibrator.isCommonAxisUsed());
3412         assertSame(calibrator.getListener(), this);
3413         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3414         assertFalse(calibrator.isReady());
3415         assertFalse(calibrator.isRunning());
3416         assertNull(calibrator.getEstimatedBiases());
3417         assertFalse(calibrator.getEstimatedBiases(null));
3418         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3419         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3420         assertNull(calibrator.getEstimatedBiasFx());
3421         assertNull(calibrator.getEstimatedBiasFy());
3422         assertNull(calibrator.getEstimatedBiasFz());
3423         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3424         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3425         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3426         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3427         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3428         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3429         assertNull(calibrator.getEstimatedMa());
3430         assertNull(calibrator.getEstimatedSx());
3431         assertNull(calibrator.getEstimatedSy());
3432         assertNull(calibrator.getEstimatedSz());
3433         assertNull(calibrator.getEstimatedMxy());
3434         assertNull(calibrator.getEstimatedMxz());
3435         assertNull(calibrator.getEstimatedMyx());
3436         assertNull(calibrator.getEstimatedMyz());
3437         assertNull(calibrator.getEstimatedMzx());
3438         assertNull(calibrator.getEstimatedMzy());
3439         assertNull(calibrator.getEstimatedCovariance());
3440         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3441     }
3442 
3443     @Test
3444     public void testConstructor34() throws WrongSizeException {
3445         final Collection<StandardDeviationBodyKinematics> measurements =
3446                 Collections.emptyList();
3447 
3448         final Matrix ba = generateBa();
3449         final double biasX = ba.getElementAtIndex(0);
3450         final double biasY = ba.getElementAtIndex(1);
3451         final double biasZ = ba.getElementAtIndex(2);
3452 
3453         final Matrix ma = generateMaCommonAxis();
3454         final double sx = ma.getElementAt(0, 0);
3455         final double sy = ma.getElementAt(1, 1);
3456         final double sz = ma.getElementAt(2, 2);
3457 
3458         final Acceleration bx = new Acceleration(biasX,
3459                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3460         final Acceleration by = new Acceleration(biasY,
3461                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3462         final Acceleration bz = new Acceleration(biasZ,
3463                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3464 
3465         final KnownPositionAccelerometerCalibrator calibrator =
3466                 new KnownPositionAccelerometerCalibrator(measurements,
3467                         bx, by, bz, sx, sy, sz);
3468 
3469         // check default values
3470         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3471         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3472         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3473         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3474         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3475         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3476         final Acceleration bx2 = new Acceleration(0.0,
3477                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3478         calibrator.getInitialBiasXAsAcceleration(bx2);
3479         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3480         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3481         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3482         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3483         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3484         final Acceleration by2 = new Acceleration(0.0,
3485                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3486         calibrator.getInitialBiasYAsAcceleration(by2);
3487         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3488         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3489         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3490         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3491         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492         final Acceleration bz2 = new Acceleration(0.0,
3493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3494         calibrator.getInitialBiasZAsAcceleration(bz2);
3495         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3496         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3497         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3498         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3499         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3500         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3501         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3502         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3503         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3504         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3505         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3506         final double[] bias1 = calibrator.getInitialBias();
3507         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3508         final double[] bias2 = new double[3];
3509         calibrator.getInitialBias(bias2);
3510         assertArrayEquals(bias1, bias2, 0.0);
3511         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3512         assertEquals(b1, ba);
3513         final Matrix b2 = new Matrix(3, 1);
3514         calibrator.getInitialBiasAsMatrix(b2);
3515         assertEquals(b1, b2);
3516         final Matrix ma1 = calibrator.getInitialMa();
3517         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3518         final Matrix ma2 = new Matrix(3, 3);
3519         calibrator.getInitialMa(ma2);
3520         assertEquals(ma1, ma2);
3521         assertNull(calibrator.getEcefPosition());
3522         assertNull(calibrator.getNedPosition());
3523         assertFalse(calibrator.getNedPosition(null));
3524         assertSame(calibrator.getMeasurements(), measurements);
3525         assertFalse(calibrator.isCommonAxisUsed());
3526         assertNull(calibrator.getListener());
3527         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3528         assertFalse(calibrator.isReady());
3529         assertFalse(calibrator.isRunning());
3530         assertNull(calibrator.getEstimatedBiases());
3531         assertFalse(calibrator.getEstimatedBiases(null));
3532         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3533         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3534         assertNull(calibrator.getEstimatedBiasFx());
3535         assertNull(calibrator.getEstimatedBiasFy());
3536         assertNull(calibrator.getEstimatedBiasFz());
3537         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3538         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3539         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3540         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3541         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3542         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3543         assertNull(calibrator.getEstimatedMa());
3544         assertNull(calibrator.getEstimatedSx());
3545         assertNull(calibrator.getEstimatedSy());
3546         assertNull(calibrator.getEstimatedSz());
3547         assertNull(calibrator.getEstimatedMxy());
3548         assertNull(calibrator.getEstimatedMxz());
3549         assertNull(calibrator.getEstimatedMyx());
3550         assertNull(calibrator.getEstimatedMyz());
3551         assertNull(calibrator.getEstimatedMzx());
3552         assertNull(calibrator.getEstimatedMzy());
3553         assertNull(calibrator.getEstimatedCovariance());
3554         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3555     }
3556 
3557     @Test
3558     public void testConstructor35() throws WrongSizeException {
3559         final Collection<StandardDeviationBodyKinematics> measurements =
3560                 Collections.emptyList();
3561 
3562         final Matrix ba = generateBa();
3563         final double biasX = ba.getElementAtIndex(0);
3564         final double biasY = ba.getElementAtIndex(1);
3565         final double biasZ = ba.getElementAtIndex(2);
3566 
3567         final Matrix ma = generateMaCommonAxis();
3568         final double sx = ma.getElementAt(0, 0);
3569         final double sy = ma.getElementAt(1, 1);
3570         final double sz = ma.getElementAt(2, 2);
3571 
3572         final Acceleration bx = new Acceleration(biasX,
3573                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3574         final Acceleration by = new Acceleration(biasY,
3575                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3576         final Acceleration bz = new Acceleration(biasZ,
3577                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3578 
3579         final KnownPositionAccelerometerCalibrator calibrator =
3580                 new KnownPositionAccelerometerCalibrator(measurements,
3581                         bx, by, bz, sx, sy, sz, this);
3582 
3583         // check default values
3584         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3585         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3586         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3587         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3588         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3589         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3590         final Acceleration bx2 = new Acceleration(0.0,
3591                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3592         calibrator.getInitialBiasXAsAcceleration(bx2);
3593         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3594         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3596         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3597         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3598         final Acceleration by2 = new Acceleration(0.0,
3599                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3600         calibrator.getInitialBiasYAsAcceleration(by2);
3601         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3602         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3603         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3604         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3605         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3606         final Acceleration bz2 = new Acceleration(0.0,
3607                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3608         calibrator.getInitialBiasZAsAcceleration(bz2);
3609         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3610         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3611         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3612         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3613         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3614         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3615         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3616         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3617         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3618         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3619         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3620         final double[] bias1 = calibrator.getInitialBias();
3621         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3622         final double[] bias2 = new double[3];
3623         calibrator.getInitialBias(bias2);
3624         assertArrayEquals(bias1, bias2, 0.0);
3625         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3626         assertEquals(b1, ba);
3627         final Matrix b2 = new Matrix(3, 1);
3628         calibrator.getInitialBiasAsMatrix(b2);
3629         assertEquals(b1, b2);
3630         final Matrix ma1 = calibrator.getInitialMa();
3631         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3632         final Matrix ma2 = new Matrix(3, 3);
3633         calibrator.getInitialMa(ma2);
3634         assertEquals(ma1, ma2);
3635         assertNull(calibrator.getEcefPosition());
3636         assertNull(calibrator.getNedPosition());
3637         assertFalse(calibrator.getNedPosition(null));
3638         assertSame(calibrator.getMeasurements(), measurements);
3639         assertFalse(calibrator.isCommonAxisUsed());
3640         assertSame(calibrator.getListener(), this);
3641         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3642         assertFalse(calibrator.isReady());
3643         assertFalse(calibrator.isRunning());
3644         assertNull(calibrator.getEstimatedBiases());
3645         assertFalse(calibrator.getEstimatedBiases(null));
3646         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3647         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3648         assertNull(calibrator.getEstimatedBiasFx());
3649         assertNull(calibrator.getEstimatedBiasFy());
3650         assertNull(calibrator.getEstimatedBiasFz());
3651         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3652         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3653         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3654         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3655         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3656         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3657         assertNull(calibrator.getEstimatedMa());
3658         assertNull(calibrator.getEstimatedSx());
3659         assertNull(calibrator.getEstimatedSy());
3660         assertNull(calibrator.getEstimatedSz());
3661         assertNull(calibrator.getEstimatedMxy());
3662         assertNull(calibrator.getEstimatedMxz());
3663         assertNull(calibrator.getEstimatedMyx());
3664         assertNull(calibrator.getEstimatedMyz());
3665         assertNull(calibrator.getEstimatedMzx());
3666         assertNull(calibrator.getEstimatedMzy());
3667         assertNull(calibrator.getEstimatedCovariance());
3668         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3669     }
3670 
3671     @Test
3672     public void testConstructor36() throws WrongSizeException {
3673         final Matrix ba = generateBa();
3674         final double biasX = ba.getElementAtIndex(0);
3675         final double biasY = ba.getElementAtIndex(1);
3676         final double biasZ = ba.getElementAtIndex(2);
3677 
3678         final Matrix ma = generateMaCommonAxis();
3679         final double sx = ma.getElementAt(0, 0);
3680         final double sy = ma.getElementAt(1, 1);
3681         final double sz = ma.getElementAt(2, 2);
3682 
3683         final Acceleration bx = new Acceleration(biasX,
3684                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3685         final Acceleration by = new Acceleration(biasY,
3686                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3687         final Acceleration bz = new Acceleration(biasZ,
3688                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3689 
3690         final KnownPositionAccelerometerCalibrator calibrator =
3691                 new KnownPositionAccelerometerCalibrator(true,
3692                         bx, by, bz, sx, sy, sz);
3693 
3694         // check default values
3695         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3696         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3697         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3698         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3699         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3700         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701         final Acceleration bx2 = new Acceleration(0.0,
3702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3703         calibrator.getInitialBiasXAsAcceleration(bx2);
3704         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3705         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3706         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3707         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3708         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709         final Acceleration by2 = new Acceleration(0.0,
3710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3711         calibrator.getInitialBiasYAsAcceleration(by2);
3712         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3713         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3714         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3715         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3716         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717         final Acceleration bz2 = new Acceleration(0.0,
3718                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3719         calibrator.getInitialBiasZAsAcceleration(bz2);
3720         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3721         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3722         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3723         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3724         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3725         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3726         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3727         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3728         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3729         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3730         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3731         final double[] bias1 = calibrator.getInitialBias();
3732         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3733         final double[] bias2 = new double[3];
3734         calibrator.getInitialBias(bias2);
3735         assertArrayEquals(bias1, bias2, 0.0);
3736         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3737         assertEquals(b1, ba);
3738         final Matrix b2 = new Matrix(3, 1);
3739         calibrator.getInitialBiasAsMatrix(b2);
3740         assertEquals(b1, b2);
3741         final Matrix ma1 = calibrator.getInitialMa();
3742         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3743         final Matrix ma2 = new Matrix(3, 3);
3744         calibrator.getInitialMa(ma2);
3745         assertEquals(ma1, ma2);
3746         assertNull(calibrator.getEcefPosition());
3747         assertNull(calibrator.getNedPosition());
3748         assertFalse(calibrator.getNedPosition(null));
3749         assertNull(calibrator.getMeasurements());
3750         assertTrue(calibrator.isCommonAxisUsed());
3751         assertNull(calibrator.getListener());
3752         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3753         assertFalse(calibrator.isReady());
3754         assertFalse(calibrator.isRunning());
3755         assertNull(calibrator.getEstimatedBiases());
3756         assertFalse(calibrator.getEstimatedBiases(null));
3757         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3758         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3759         assertNull(calibrator.getEstimatedBiasFx());
3760         assertNull(calibrator.getEstimatedBiasFy());
3761         assertNull(calibrator.getEstimatedBiasFz());
3762         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3763         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3764         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3765         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3766         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3767         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3768         assertNull(calibrator.getEstimatedMa());
3769         assertNull(calibrator.getEstimatedSx());
3770         assertNull(calibrator.getEstimatedSy());
3771         assertNull(calibrator.getEstimatedSz());
3772         assertNull(calibrator.getEstimatedMxy());
3773         assertNull(calibrator.getEstimatedMxz());
3774         assertNull(calibrator.getEstimatedMyx());
3775         assertNull(calibrator.getEstimatedMyz());
3776         assertNull(calibrator.getEstimatedMzx());
3777         assertNull(calibrator.getEstimatedMzy());
3778         assertNull(calibrator.getEstimatedCovariance());
3779         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3780     }
3781 
3782     @Test
3783     public void testConstructor37() throws WrongSizeException {
3784         final Matrix ba = generateBa();
3785         final double biasX = ba.getElementAtIndex(0);
3786         final double biasY = ba.getElementAtIndex(1);
3787         final double biasZ = ba.getElementAtIndex(2);
3788 
3789         final Matrix ma = generateMaCommonAxis();
3790         final double sx = ma.getElementAt(0, 0);
3791         final double sy = ma.getElementAt(1, 1);
3792         final double sz = ma.getElementAt(2, 2);
3793 
3794         final Acceleration bx = new Acceleration(biasX,
3795                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3796         final Acceleration by = new Acceleration(biasY,
3797                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3798         final Acceleration bz = new Acceleration(biasZ,
3799                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3800 
3801         final KnownPositionAccelerometerCalibrator calibrator =
3802                 new KnownPositionAccelerometerCalibrator(true,
3803                         bx, by, bz, sx, sy, sz, this);
3804 
3805         // check default values
3806         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3807         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3808         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3809         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3810         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3811         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3812         final Acceleration bx2 = new Acceleration(0.0,
3813                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3814         calibrator.getInitialBiasXAsAcceleration(bx2);
3815         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3816         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3817         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3818         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3819         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3820         final Acceleration by2 = new Acceleration(0.0,
3821                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3822         calibrator.getInitialBiasYAsAcceleration(by2);
3823         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3824         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3826         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3827         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3828         final Acceleration bz2 = new Acceleration(0.0,
3829                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3830         calibrator.getInitialBiasZAsAcceleration(bz2);
3831         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3832         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3834         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3835         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3836         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3837         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3838         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3839         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3840         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3841         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3842         final double[] bias1 = calibrator.getInitialBias();
3843         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3844         final double[] bias2 = new double[3];
3845         calibrator.getInitialBias(bias2);
3846         assertArrayEquals(bias1, bias2, 0.0);
3847         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3848         assertEquals(b1, ba);
3849         final Matrix b2 = new Matrix(3, 1);
3850         calibrator.getInitialBiasAsMatrix(b2);
3851         assertEquals(b1, b2);
3852         final Matrix ma1 = calibrator.getInitialMa();
3853         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3854         final Matrix ma2 = new Matrix(3, 3);
3855         calibrator.getInitialMa(ma2);
3856         assertEquals(ma1, ma2);
3857         assertNull(calibrator.getEcefPosition());
3858         assertNull(calibrator.getNedPosition());
3859         assertFalse(calibrator.getNedPosition(null));
3860         assertNull(calibrator.getMeasurements());
3861         assertTrue(calibrator.isCommonAxisUsed());
3862         assertSame(calibrator.getListener(), this);
3863         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3864         assertFalse(calibrator.isReady());
3865         assertFalse(calibrator.isRunning());
3866         assertNull(calibrator.getEstimatedBiases());
3867         assertFalse(calibrator.getEstimatedBiases(null));
3868         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3869         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3870         assertNull(calibrator.getEstimatedBiasFx());
3871         assertNull(calibrator.getEstimatedBiasFy());
3872         assertNull(calibrator.getEstimatedBiasFz());
3873         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3874         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3875         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3876         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3877         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3878         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3879         assertNull(calibrator.getEstimatedMa());
3880         assertNull(calibrator.getEstimatedSx());
3881         assertNull(calibrator.getEstimatedSy());
3882         assertNull(calibrator.getEstimatedSz());
3883         assertNull(calibrator.getEstimatedMxy());
3884         assertNull(calibrator.getEstimatedMxz());
3885         assertNull(calibrator.getEstimatedMyx());
3886         assertNull(calibrator.getEstimatedMyz());
3887         assertNull(calibrator.getEstimatedMzx());
3888         assertNull(calibrator.getEstimatedMzy());
3889         assertNull(calibrator.getEstimatedCovariance());
3890         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3891     }
3892 
3893     @Test
3894     public void testConstructor38() throws WrongSizeException {
3895         final Collection<StandardDeviationBodyKinematics> measurements =
3896                 Collections.emptyList();
3897 
3898         final Matrix ba = generateBa();
3899         final double biasX = ba.getElementAtIndex(0);
3900         final double biasY = ba.getElementAtIndex(1);
3901         final double biasZ = ba.getElementAtIndex(2);
3902 
3903         final Matrix ma = generateMaCommonAxis();
3904         final double sx = ma.getElementAt(0, 0);
3905         final double sy = ma.getElementAt(1, 1);
3906         final double sz = ma.getElementAt(2, 2);
3907 
3908         final Acceleration bx = new Acceleration(biasX,
3909                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3910         final Acceleration by = new Acceleration(biasY,
3911                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3912         final Acceleration bz = new Acceleration(biasZ,
3913                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3914 
3915         final KnownPositionAccelerometerCalibrator calibrator =
3916                 new KnownPositionAccelerometerCalibrator(measurements,
3917                         true, bx, by, bz, sx, sy, sz);
3918 
3919         // check default values
3920         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3921         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3922         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3923         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3924         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3926         final Acceleration bx2 = new Acceleration(0.0,
3927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3928         calibrator.getInitialBiasXAsAcceleration(bx2);
3929         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3930         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3931         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3932         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3933         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3934         final Acceleration by2 = new Acceleration(0.0,
3935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3936         calibrator.getInitialBiasYAsAcceleration(by2);
3937         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3938         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3940         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3941         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3942         final Acceleration bz2 = new Acceleration(0.0,
3943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3944         calibrator.getInitialBiasZAsAcceleration(bz2);
3945         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3946         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3948         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3949         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3950         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3951         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3952         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3953         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3954         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3955         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3956         final double[] bias1 = calibrator.getInitialBias();
3957         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
3958         final double[] bias2 = new double[3];
3959         calibrator.getInitialBias(bias2);
3960         assertArrayEquals(bias1, bias2, 0.0);
3961         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3962         assertEquals(b1, ba);
3963         final Matrix b2 = new Matrix(3, 1);
3964         calibrator.getInitialBiasAsMatrix(b2);
3965         assertEquals(b1, b2);
3966         final Matrix ma1 = calibrator.getInitialMa();
3967         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
3968         final Matrix ma2 = new Matrix(3, 3);
3969         calibrator.getInitialMa(ma2);
3970         assertEquals(ma1, ma2);
3971         assertNull(calibrator.getEcefPosition());
3972         assertNull(calibrator.getNedPosition());
3973         assertFalse(calibrator.getNedPosition(null));
3974         assertSame(calibrator.getMeasurements(), measurements);
3975         assertTrue(calibrator.isCommonAxisUsed());
3976         assertNull(calibrator.getListener());
3977         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3978         assertFalse(calibrator.isReady());
3979         assertFalse(calibrator.isRunning());
3980         assertNull(calibrator.getEstimatedBiases());
3981         assertFalse(calibrator.getEstimatedBiases(null));
3982         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3983         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3984         assertNull(calibrator.getEstimatedBiasFx());
3985         assertNull(calibrator.getEstimatedBiasFy());
3986         assertNull(calibrator.getEstimatedBiasFz());
3987         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3988         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3989         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3990         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3991         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3992         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3993         assertNull(calibrator.getEstimatedMa());
3994         assertNull(calibrator.getEstimatedSx());
3995         assertNull(calibrator.getEstimatedSy());
3996         assertNull(calibrator.getEstimatedSz());
3997         assertNull(calibrator.getEstimatedMxy());
3998         assertNull(calibrator.getEstimatedMxz());
3999         assertNull(calibrator.getEstimatedMyx());
4000         assertNull(calibrator.getEstimatedMyz());
4001         assertNull(calibrator.getEstimatedMzx());
4002         assertNull(calibrator.getEstimatedMzy());
4003         assertNull(calibrator.getEstimatedCovariance());
4004         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4005     }
4006 
4007     @Test
4008     public void testConstructor39() throws WrongSizeException {
4009         final Collection<StandardDeviationBodyKinematics> measurements =
4010                 Collections.emptyList();
4011 
4012         final Matrix ba = generateBa();
4013         final double biasX = ba.getElementAtIndex(0);
4014         final double biasY = ba.getElementAtIndex(1);
4015         final double biasZ = ba.getElementAtIndex(2);
4016 
4017         final Matrix ma = generateMaCommonAxis();
4018         final double sx = ma.getElementAt(0, 0);
4019         final double sy = ma.getElementAt(1, 1);
4020         final double sz = ma.getElementAt(2, 2);
4021 
4022         final Acceleration bx = new Acceleration(biasX,
4023                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4024         final Acceleration by = new Acceleration(biasY,
4025                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4026         final Acceleration bz = new Acceleration(biasZ,
4027                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4028 
4029         final KnownPositionAccelerometerCalibrator calibrator =
4030                 new KnownPositionAccelerometerCalibrator(measurements,
4031                         true, bx, by, bz, sx, sy, sz,
4032                         this);
4033 
4034         // check default values
4035         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4036         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4037         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4038         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4039         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4040         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4041         final Acceleration bx2 = new Acceleration(0.0,
4042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4043         calibrator.getInitialBiasXAsAcceleration(bx2);
4044         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4045         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4046         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4047         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4048         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049         final Acceleration by2 = new Acceleration(0.0,
4050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051         calibrator.getInitialBiasYAsAcceleration(by2);
4052         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4053         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4055         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4056         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057         final Acceleration bz2 = new Acceleration(0.0,
4058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059         calibrator.getInitialBiasZAsAcceleration(bz2);
4060         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4061         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4063         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4064         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4065         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4066         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4067         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4068         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4069         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4070         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4071         final double[] bias1 = calibrator.getInitialBias();
4072         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4073         final double[] bias2 = new double[3];
4074         calibrator.getInitialBias(bias2);
4075         assertArrayEquals(bias1, bias2, 0.0);
4076         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4077         assertEquals(b1, ba);
4078         final Matrix b2 = new Matrix(3, 1);
4079         calibrator.getInitialBiasAsMatrix(b2);
4080         assertEquals(b1, b2);
4081         final Matrix ma1 = calibrator.getInitialMa();
4082         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
4083         final Matrix ma2 = new Matrix(3, 3);
4084         calibrator.getInitialMa(ma2);
4085         assertEquals(ma1, ma2);
4086         assertNull(calibrator.getEcefPosition());
4087         assertNull(calibrator.getNedPosition());
4088         assertFalse(calibrator.getNedPosition(null));
4089         assertSame(calibrator.getMeasurements(), measurements);
4090         assertTrue(calibrator.isCommonAxisUsed());
4091         assertSame(calibrator.getListener(), this);
4092         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4093         assertFalse(calibrator.isReady());
4094         assertFalse(calibrator.isRunning());
4095         assertNull(calibrator.getEstimatedBiases());
4096         assertFalse(calibrator.getEstimatedBiases(null));
4097         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4098         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4099         assertNull(calibrator.getEstimatedBiasFx());
4100         assertNull(calibrator.getEstimatedBiasFy());
4101         assertNull(calibrator.getEstimatedBiasFz());
4102         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4103         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4104         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4105         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4106         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4107         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4108         assertNull(calibrator.getEstimatedMa());
4109         assertNull(calibrator.getEstimatedSx());
4110         assertNull(calibrator.getEstimatedSy());
4111         assertNull(calibrator.getEstimatedSz());
4112         assertNull(calibrator.getEstimatedMxy());
4113         assertNull(calibrator.getEstimatedMxz());
4114         assertNull(calibrator.getEstimatedMyx());
4115         assertNull(calibrator.getEstimatedMyz());
4116         assertNull(calibrator.getEstimatedMzx());
4117         assertNull(calibrator.getEstimatedMzy());
4118         assertNull(calibrator.getEstimatedCovariance());
4119         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4120     }
4121 
4122     @Test
4123     public void testConstructor40() throws WrongSizeException {
4124         final Matrix ba = generateBa();
4125         final double biasX = ba.getElementAtIndex(0);
4126         final double biasY = ba.getElementAtIndex(1);
4127         final double biasZ = ba.getElementAtIndex(2);
4128 
4129         final Matrix ma = generateMaCommonAxis();
4130         final double sx = ma.getElementAt(0, 0);
4131         final double sy = ma.getElementAt(1, 1);
4132         final double sz = ma.getElementAt(2, 2);
4133         final double mxy = ma.getElementAt(0, 1);
4134         final double mxz = ma.getElementAt(0, 2);
4135         final double myx = ma.getElementAt(1, 0);
4136         final double myz = ma.getElementAt(1, 2);
4137         final double mzx = ma.getElementAt(2, 0);
4138         final double mzy = ma.getElementAt(2, 1);
4139 
4140         final KnownPositionAccelerometerCalibrator calibrator =
4141                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4142                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4143 
4144         // check default values
4145         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4146         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4147         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4148         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4149         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4150         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4151         final Acceleration bx2 = new Acceleration(0.0,
4152                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4153         calibrator.getInitialBiasXAsAcceleration(bx2);
4154         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4155         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4156         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4157         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4158         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159         final Acceleration by2 = new Acceleration(0.0,
4160                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4161         calibrator.getInitialBiasYAsAcceleration(by2);
4162         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4163         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4164         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4165         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4166         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4167         final Acceleration bz2 = new Acceleration(0.0,
4168                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4169         calibrator.getInitialBiasZAsAcceleration(bz2);
4170         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4171         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4172         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4173         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4174         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4175         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4176         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4177         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4178         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4179         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4180         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4181         final double[] bias1 = calibrator.getInitialBias();
4182         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4183         final double[] bias2 = new double[3];
4184         calibrator.getInitialBias(bias2);
4185         assertArrayEquals(bias1, bias2, 0.0);
4186         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4187         assertEquals(b1, ba);
4188         final Matrix b2 = new Matrix(3, 1);
4189         calibrator.getInitialBiasAsMatrix(b2);
4190         assertEquals(b1, b2);
4191         final Matrix ma1 = new Matrix(3, 3);
4192         ma1.setSubmatrix(0, 0,
4193                 2, 2,
4194                 new double[]{ sx, myx, mzx,
4195                         mxy, sy, mzy,
4196                         mxz, myz, sz});
4197         assertEquals(calibrator.getInitialMa(), ma1);
4198         final Matrix ma2 = new Matrix(3, 3);
4199         calibrator.getInitialMa(ma2);
4200         assertEquals(ma1, ma2);
4201         assertNull(calibrator.getEcefPosition());
4202         assertNull(calibrator.getNedPosition());
4203         assertFalse(calibrator.getNedPosition(null));
4204         assertNull(calibrator.getMeasurements());
4205         assertFalse(calibrator.isCommonAxisUsed());
4206         assertNull(calibrator.getListener());
4207         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4208         assertFalse(calibrator.isReady());
4209         assertFalse(calibrator.isRunning());
4210         assertNull(calibrator.getEstimatedBiases());
4211         assertFalse(calibrator.getEstimatedBiases(null));
4212         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4213         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4214         assertNull(calibrator.getEstimatedBiasFx());
4215         assertNull(calibrator.getEstimatedBiasFy());
4216         assertNull(calibrator.getEstimatedBiasFz());
4217         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4218         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4219         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4220         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4221         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4222         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4223         assertNull(calibrator.getEstimatedMa());
4224         assertNull(calibrator.getEstimatedSx());
4225         assertNull(calibrator.getEstimatedSy());
4226         assertNull(calibrator.getEstimatedSz());
4227         assertNull(calibrator.getEstimatedMxy());
4228         assertNull(calibrator.getEstimatedMxz());
4229         assertNull(calibrator.getEstimatedMyx());
4230         assertNull(calibrator.getEstimatedMyz());
4231         assertNull(calibrator.getEstimatedMzx());
4232         assertNull(calibrator.getEstimatedMzy());
4233         assertNull(calibrator.getEstimatedCovariance());
4234         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4235     }
4236 
4237     @Test
4238     public void testConstructor41() throws WrongSizeException {
4239         final Collection<StandardDeviationBodyKinematics> measurements =
4240                 Collections.emptyList();
4241 
4242         final Matrix ba = generateBa();
4243         final double biasX = ba.getElementAtIndex(0);
4244         final double biasY = ba.getElementAtIndex(1);
4245         final double biasZ = ba.getElementAtIndex(2);
4246 
4247         final Matrix ma = generateMaCommonAxis();
4248         final double sx = ma.getElementAt(0, 0);
4249         final double sy = ma.getElementAt(1, 1);
4250         final double sz = ma.getElementAt(2, 2);
4251         final double mxy = ma.getElementAt(0, 1);
4252         final double mxz = ma.getElementAt(0, 2);
4253         final double myx = ma.getElementAt(1, 0);
4254         final double myz = ma.getElementAt(1, 2);
4255         final double mzx = ma.getElementAt(2, 0);
4256         final double mzy = ma.getElementAt(2, 1);
4257 
4258         final KnownPositionAccelerometerCalibrator calibrator =
4259                 new KnownPositionAccelerometerCalibrator(measurements,
4260                         biasX, biasY, biasZ, sx, sy, sz,
4261                         mxy, mxz, myx, myz, mzx, mzy);
4262 
4263         // check default values
4264         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4265         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4266         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4267         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4268         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4269         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4270         final Acceleration bx2 = new Acceleration(0.0,
4271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4272         calibrator.getInitialBiasXAsAcceleration(bx2);
4273         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4274         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4275         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4276         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4277         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4278         final Acceleration by2 = new Acceleration(0.0,
4279                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4280         calibrator.getInitialBiasYAsAcceleration(by2);
4281         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4282         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4283         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4284         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4285         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4286         final Acceleration bz2 = new Acceleration(0.0,
4287                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4288         calibrator.getInitialBiasZAsAcceleration(bz2);
4289         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4290         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4291         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4292         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4293         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4294         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4295         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4296         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4297         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4298         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4299         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4300         final double[] bias1 = calibrator.getInitialBias();
4301         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4302         final double[] bias2 = new double[3];
4303         calibrator.getInitialBias(bias2);
4304         assertArrayEquals(bias1, bias2, 0.0);
4305         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4306         assertEquals(b1, ba);
4307         final Matrix b2 = new Matrix(3, 1);
4308         calibrator.getInitialBiasAsMatrix(b2);
4309         assertEquals(b1, b2);
4310         final Matrix ma1 = new Matrix(3, 3);
4311         ma1.setSubmatrix(0, 0,
4312                 2, 2,
4313                 new double[]{ sx, myx, mzx,
4314                         mxy, sy, mzy,
4315                         mxz, myz, sz});
4316         assertEquals(calibrator.getInitialMa(), ma1);
4317         final Matrix ma2 = new Matrix(3, 3);
4318         calibrator.getInitialMa(ma2);
4319         assertEquals(ma1, ma2);
4320         assertNull(calibrator.getEcefPosition());
4321         assertNull(calibrator.getNedPosition());
4322         assertFalse(calibrator.getNedPosition(null));
4323         assertSame(calibrator.getMeasurements(), measurements);
4324         assertFalse(calibrator.isCommonAxisUsed());
4325         assertNull(calibrator.getListener());
4326         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4327         assertFalse(calibrator.isReady());
4328         assertFalse(calibrator.isRunning());
4329         assertNull(calibrator.getEstimatedBiases());
4330         assertFalse(calibrator.getEstimatedBiases(null));
4331         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4332         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4333         assertNull(calibrator.getEstimatedBiasFx());
4334         assertNull(calibrator.getEstimatedBiasFy());
4335         assertNull(calibrator.getEstimatedBiasFz());
4336         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4337         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4338         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4339         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4340         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4341         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4342         assertNull(calibrator.getEstimatedMa());
4343         assertNull(calibrator.getEstimatedSx());
4344         assertNull(calibrator.getEstimatedSy());
4345         assertNull(calibrator.getEstimatedSz());
4346         assertNull(calibrator.getEstimatedMxy());
4347         assertNull(calibrator.getEstimatedMxz());
4348         assertNull(calibrator.getEstimatedMyx());
4349         assertNull(calibrator.getEstimatedMyz());
4350         assertNull(calibrator.getEstimatedMzx());
4351         assertNull(calibrator.getEstimatedMzy());
4352         assertNull(calibrator.getEstimatedCovariance());
4353         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4354     }
4355 
4356     @Test
4357     public void testConstructor42() throws WrongSizeException {
4358         final Collection<StandardDeviationBodyKinematics> measurements =
4359                 Collections.emptyList();
4360 
4361         final Matrix ba = generateBa();
4362         final double biasX = ba.getElementAtIndex(0);
4363         final double biasY = ba.getElementAtIndex(1);
4364         final double biasZ = ba.getElementAtIndex(2);
4365 
4366         final Matrix ma = generateMaCommonAxis();
4367         final double sx = ma.getElementAt(0, 0);
4368         final double sy = ma.getElementAt(1, 1);
4369         final double sz = ma.getElementAt(2, 2);
4370         final double mxy = ma.getElementAt(0, 1);
4371         final double mxz = ma.getElementAt(0, 2);
4372         final double myx = ma.getElementAt(1, 0);
4373         final double myz = ma.getElementAt(1, 2);
4374         final double mzx = ma.getElementAt(2, 0);
4375         final double mzy = ma.getElementAt(2, 1);
4376 
4377         final KnownPositionAccelerometerCalibrator calibrator =
4378                 new KnownPositionAccelerometerCalibrator(measurements,
4379                         biasX, biasY, biasZ, sx, sy, sz,
4380                         mxy, mxz, myx, myz, mzx, mzy, this);
4381 
4382         // check default values
4383         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4384         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4385         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4386         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4387         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4388         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4389         final Acceleration bx2 = new Acceleration(0.0,
4390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4391         calibrator.getInitialBiasXAsAcceleration(bx2);
4392         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4393         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4395         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4396         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4397         final Acceleration by2 = new Acceleration(0.0,
4398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4399         calibrator.getInitialBiasYAsAcceleration(by2);
4400         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4401         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4402         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4403         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4404         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4405         final Acceleration bz2 = new Acceleration(0.0,
4406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4407         calibrator.getInitialBiasZAsAcceleration(bz2);
4408         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4409         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4410         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4411         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4412         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4413         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4414         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4415         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4416         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4417         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4418         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4419         final double[] bias1 = calibrator.getInitialBias();
4420         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4421         final double[] bias2 = new double[3];
4422         calibrator.getInitialBias(bias2);
4423         assertArrayEquals(bias1, bias2, 0.0);
4424         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4425         assertEquals(b1, ba);
4426         final Matrix b2 = new Matrix(3, 1);
4427         calibrator.getInitialBiasAsMatrix(b2);
4428         assertEquals(b1, b2);
4429         final Matrix ma1 = new Matrix(3, 3);
4430         ma1.setSubmatrix(0, 0,
4431                 2, 2,
4432                 new double[]{ sx, myx, mzx,
4433                         mxy, sy, mzy,
4434                         mxz, myz, sz});
4435         assertEquals(calibrator.getInitialMa(), ma1);
4436         final Matrix ma2 = new Matrix(3, 3);
4437         calibrator.getInitialMa(ma2);
4438         assertEquals(ma1, ma2);
4439         assertNull(calibrator.getEcefPosition());
4440         assertNull(calibrator.getNedPosition());
4441         assertFalse(calibrator.getNedPosition(null));
4442         assertSame(calibrator.getMeasurements(), measurements);
4443         assertFalse(calibrator.isCommonAxisUsed());
4444         assertSame(calibrator.getListener(), this);
4445         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4446         assertFalse(calibrator.isReady());
4447         assertFalse(calibrator.isRunning());
4448         assertNull(calibrator.getEstimatedBiases());
4449         assertFalse(calibrator.getEstimatedBiases(null));
4450         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4451         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4452         assertNull(calibrator.getEstimatedBiasFx());
4453         assertNull(calibrator.getEstimatedBiasFy());
4454         assertNull(calibrator.getEstimatedBiasFz());
4455         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4456         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4457         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4458         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4459         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4460         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4461         assertNull(calibrator.getEstimatedMa());
4462         assertNull(calibrator.getEstimatedSx());
4463         assertNull(calibrator.getEstimatedSy());
4464         assertNull(calibrator.getEstimatedSz());
4465         assertNull(calibrator.getEstimatedMxy());
4466         assertNull(calibrator.getEstimatedMxz());
4467         assertNull(calibrator.getEstimatedMyx());
4468         assertNull(calibrator.getEstimatedMyz());
4469         assertNull(calibrator.getEstimatedMzx());
4470         assertNull(calibrator.getEstimatedMzy());
4471         assertNull(calibrator.getEstimatedCovariance());
4472         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4473     }
4474 
4475     @Test
4476     public void testConstructor43() throws WrongSizeException {
4477         final Matrix ba = generateBa();
4478         final double biasX = ba.getElementAtIndex(0);
4479         final double biasY = ba.getElementAtIndex(1);
4480         final double biasZ = ba.getElementAtIndex(2);
4481 
4482         final Matrix ma = generateMaCommonAxis();
4483         final double sx = ma.getElementAt(0, 0);
4484         final double sy = ma.getElementAt(1, 1);
4485         final double sz = ma.getElementAt(2, 2);
4486         final double mxy = ma.getElementAt(0, 1);
4487         final double mxz = ma.getElementAt(0, 2);
4488         final double myx = ma.getElementAt(1, 0);
4489         final double myz = ma.getElementAt(1, 2);
4490         final double mzx = ma.getElementAt(2, 0);
4491         final double mzy = ma.getElementAt(2, 1);
4492 
4493         final KnownPositionAccelerometerCalibrator calibrator =
4494                 new KnownPositionAccelerometerCalibrator(true,
4495                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4496                         myz, mzx, mzy);
4497 
4498         // check default values
4499         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4500         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4501         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4502         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4503         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4504         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4505         final Acceleration bx2 = new Acceleration(0.0,
4506                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4507         calibrator.getInitialBiasXAsAcceleration(bx2);
4508         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4509         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4510         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4511         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4512         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4513         final Acceleration by2 = new Acceleration(0.0,
4514                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4515         calibrator.getInitialBiasYAsAcceleration(by2);
4516         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4517         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4518         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4519         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4520         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4521         final Acceleration bz2 = new Acceleration(0.0,
4522                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4523         calibrator.getInitialBiasZAsAcceleration(bz2);
4524         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4525         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4526         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4527         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4528         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4529         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4530         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4531         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4532         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4533         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4534         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4535         final double[] bias1 = calibrator.getInitialBias();
4536         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4537         final double[] bias2 = new double[3];
4538         calibrator.getInitialBias(bias2);
4539         assertArrayEquals(bias1, bias2, 0.0);
4540         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4541         assertEquals(b1, ba);
4542         final Matrix b2 = new Matrix(3, 1);
4543         calibrator.getInitialBiasAsMatrix(b2);
4544         assertEquals(b1, b2);
4545         final Matrix ma1 = new Matrix(3, 3);
4546         ma1.setSubmatrix(0, 0,
4547                 2, 2,
4548                 new double[]{ sx, myx, mzx,
4549                         mxy, sy, mzy,
4550                         mxz, myz, sz});
4551         assertEquals(calibrator.getInitialMa(), ma1);
4552         final Matrix ma2 = new Matrix(3, 3);
4553         calibrator.getInitialMa(ma2);
4554         assertEquals(ma1, ma2);
4555         assertNull(calibrator.getEcefPosition());
4556         assertNull(calibrator.getNedPosition());
4557         assertFalse(calibrator.getNedPosition(null));
4558         assertNull(calibrator.getMeasurements());
4559         assertTrue(calibrator.isCommonAxisUsed());
4560         assertNull(calibrator.getListener());
4561         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4562         assertFalse(calibrator.isReady());
4563         assertFalse(calibrator.isRunning());
4564         assertNull(calibrator.getEstimatedBiases());
4565         assertFalse(calibrator.getEstimatedBiases(null));
4566         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4567         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4568         assertNull(calibrator.getEstimatedBiasFx());
4569         assertNull(calibrator.getEstimatedBiasFy());
4570         assertNull(calibrator.getEstimatedBiasFz());
4571         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4572         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4573         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4574         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4575         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4576         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4577         assertNull(calibrator.getEstimatedMa());
4578         assertNull(calibrator.getEstimatedSx());
4579         assertNull(calibrator.getEstimatedSy());
4580         assertNull(calibrator.getEstimatedSz());
4581         assertNull(calibrator.getEstimatedMxy());
4582         assertNull(calibrator.getEstimatedMxz());
4583         assertNull(calibrator.getEstimatedMyx());
4584         assertNull(calibrator.getEstimatedMyz());
4585         assertNull(calibrator.getEstimatedMzx());
4586         assertNull(calibrator.getEstimatedMzy());
4587         assertNull(calibrator.getEstimatedCovariance());
4588         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4589     }
4590 
4591     @Test
4592     public void testConstructor44() throws WrongSizeException {
4593         final Matrix ba = generateBa();
4594         final double biasX = ba.getElementAtIndex(0);
4595         final double biasY = ba.getElementAtIndex(1);
4596         final double biasZ = ba.getElementAtIndex(2);
4597 
4598         final Matrix ma = generateMaCommonAxis();
4599         final double sx = ma.getElementAt(0, 0);
4600         final double sy = ma.getElementAt(1, 1);
4601         final double sz = ma.getElementAt(2, 2);
4602         final double mxy = ma.getElementAt(0, 1);
4603         final double mxz = ma.getElementAt(0, 2);
4604         final double myx = ma.getElementAt(1, 0);
4605         final double myz = ma.getElementAt(1, 2);
4606         final double mzx = ma.getElementAt(2, 0);
4607         final double mzy = ma.getElementAt(2, 1);
4608 
4609         final KnownPositionAccelerometerCalibrator calibrator =
4610                 new KnownPositionAccelerometerCalibrator(true,
4611                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4612                         myz, mzx, mzy, this);
4613 
4614         // check default values
4615         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4616         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4617         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4618         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4619         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4620         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4621         final Acceleration bx2 = new Acceleration(0.0,
4622                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4623         calibrator.getInitialBiasXAsAcceleration(bx2);
4624         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4625         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4626         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4627         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4628         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4629         final Acceleration by2 = new Acceleration(0.0,
4630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4631         calibrator.getInitialBiasYAsAcceleration(by2);
4632         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4633         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4634         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4635         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4636         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4637         final Acceleration bz2 = new Acceleration(0.0,
4638                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4639         calibrator.getInitialBiasZAsAcceleration(bz2);
4640         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4641         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4642         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4643         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4644         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4645         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4646         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4647         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4648         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4649         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4650         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4651         final double[] bias1 = calibrator.getInitialBias();
4652         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4653         final double[] bias2 = new double[3];
4654         calibrator.getInitialBias(bias2);
4655         assertArrayEquals(bias1, bias2, 0.0);
4656         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4657         assertEquals(b1, ba);
4658         final Matrix b2 = new Matrix(3, 1);
4659         calibrator.getInitialBiasAsMatrix(b2);
4660         assertEquals(b1, b2);
4661         final Matrix ma1 = new Matrix(3, 3);
4662         ma1.setSubmatrix(0, 0,
4663                 2, 2,
4664                 new double[]{ sx, myx, mzx,
4665                         mxy, sy, mzy,
4666                         mxz, myz, sz});
4667         assertEquals(calibrator.getInitialMa(), ma1);
4668         final Matrix ma2 = new Matrix(3, 3);
4669         calibrator.getInitialMa(ma2);
4670         assertEquals(ma1, ma2);
4671         assertNull(calibrator.getEcefPosition());
4672         assertNull(calibrator.getNedPosition());
4673         assertFalse(calibrator.getNedPosition(null));
4674         assertNull(calibrator.getMeasurements());
4675         assertTrue(calibrator.isCommonAxisUsed());
4676         assertSame(calibrator.getListener(), this);
4677         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4678         assertFalse(calibrator.isReady());
4679         assertFalse(calibrator.isRunning());
4680         assertNull(calibrator.getEstimatedBiases());
4681         assertFalse(calibrator.getEstimatedBiases(null));
4682         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4683         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4684         assertNull(calibrator.getEstimatedBiasFx());
4685         assertNull(calibrator.getEstimatedBiasFy());
4686         assertNull(calibrator.getEstimatedBiasFz());
4687         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4688         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4689         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4690         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4691         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4692         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4693         assertNull(calibrator.getEstimatedMa());
4694         assertNull(calibrator.getEstimatedSx());
4695         assertNull(calibrator.getEstimatedSy());
4696         assertNull(calibrator.getEstimatedSz());
4697         assertNull(calibrator.getEstimatedMxy());
4698         assertNull(calibrator.getEstimatedMxz());
4699         assertNull(calibrator.getEstimatedMyx());
4700         assertNull(calibrator.getEstimatedMyz());
4701         assertNull(calibrator.getEstimatedMzx());
4702         assertNull(calibrator.getEstimatedMzy());
4703         assertNull(calibrator.getEstimatedCovariance());
4704         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4705     }
4706 
4707     @Test
4708     public void testConstructor45() throws WrongSizeException {
4709         final Collection<StandardDeviationBodyKinematics> measurements =
4710                 Collections.emptyList();
4711 
4712         final Matrix ba = generateBa();
4713         final double biasX = ba.getElementAtIndex(0);
4714         final double biasY = ba.getElementAtIndex(1);
4715         final double biasZ = ba.getElementAtIndex(2);
4716 
4717         final Matrix ma = generateMaCommonAxis();
4718         final double sx = ma.getElementAt(0, 0);
4719         final double sy = ma.getElementAt(1, 1);
4720         final double sz = ma.getElementAt(2, 2);
4721         final double mxy = ma.getElementAt(0, 1);
4722         final double mxz = ma.getElementAt(0, 2);
4723         final double myx = ma.getElementAt(1, 0);
4724         final double myz = ma.getElementAt(1, 2);
4725         final double mzx = ma.getElementAt(2, 0);
4726         final double mzy = ma.getElementAt(2, 1);
4727 
4728         final KnownPositionAccelerometerCalibrator calibrator =
4729                 new KnownPositionAccelerometerCalibrator(measurements,
4730                         true, biasX, biasY, biasZ,
4731                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4732 
4733         // check default values
4734         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4735         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4736         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4737         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4738         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4739         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4740         final Acceleration bx2 = new Acceleration(0.0,
4741                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4742         calibrator.getInitialBiasXAsAcceleration(bx2);
4743         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4744         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4745         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4746         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4747         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4748         final Acceleration by2 = new Acceleration(0.0,
4749                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4750         calibrator.getInitialBiasYAsAcceleration(by2);
4751         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4752         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4753         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4754         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4755         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4756         final Acceleration bz2 = new Acceleration(0.0,
4757                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4758         calibrator.getInitialBiasZAsAcceleration(bz2);
4759         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4760         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4761         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4762         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4763         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4764         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4765         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4766         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4767         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4768         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4769         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4770         final double[] bias1 = calibrator.getInitialBias();
4771         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4772         final double[] bias2 = new double[3];
4773         calibrator.getInitialBias(bias2);
4774         assertArrayEquals(bias1, bias2, 0.0);
4775         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4776         assertEquals(b1, ba);
4777         final Matrix b2 = new Matrix(3, 1);
4778         calibrator.getInitialBiasAsMatrix(b2);
4779         assertEquals(b1, b2);
4780         final Matrix ma1 = new Matrix(3, 3);
4781         ma1.setSubmatrix(0, 0,
4782                 2, 2,
4783                 new double[]{ sx, myx, mzx,
4784                         mxy, sy, mzy,
4785                         mxz, myz, sz});
4786         assertEquals(calibrator.getInitialMa(), ma1);
4787         final Matrix ma2 = new Matrix(3, 3);
4788         calibrator.getInitialMa(ma2);
4789         assertEquals(ma1, ma2);
4790         assertNull(calibrator.getEcefPosition());
4791         assertNull(calibrator.getNedPosition());
4792         assertFalse(calibrator.getNedPosition(null));
4793         assertSame(calibrator.getMeasurements(), measurements);
4794         assertTrue(calibrator.isCommonAxisUsed());
4795         assertNull(calibrator.getListener());
4796         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4797         assertFalse(calibrator.isReady());
4798         assertFalse(calibrator.isRunning());
4799         assertNull(calibrator.getEstimatedBiases());
4800         assertFalse(calibrator.getEstimatedBiases(null));
4801         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4802         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4803         assertNull(calibrator.getEstimatedBiasFx());
4804         assertNull(calibrator.getEstimatedBiasFy());
4805         assertNull(calibrator.getEstimatedBiasFz());
4806         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4807         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4808         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4809         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4810         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4811         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4812         assertNull(calibrator.getEstimatedMa());
4813         assertNull(calibrator.getEstimatedSx());
4814         assertNull(calibrator.getEstimatedSy());
4815         assertNull(calibrator.getEstimatedSz());
4816         assertNull(calibrator.getEstimatedMxy());
4817         assertNull(calibrator.getEstimatedMxz());
4818         assertNull(calibrator.getEstimatedMyx());
4819         assertNull(calibrator.getEstimatedMyz());
4820         assertNull(calibrator.getEstimatedMzx());
4821         assertNull(calibrator.getEstimatedMzy());
4822         assertNull(calibrator.getEstimatedCovariance());
4823         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4824     }
4825 
4826     @Test
4827     public void testConstructor46() throws WrongSizeException {
4828         final Collection<StandardDeviationBodyKinematics> measurements =
4829                 Collections.emptyList();
4830 
4831         final Matrix ba = generateBa();
4832         final double biasX = ba.getElementAtIndex(0);
4833         final double biasY = ba.getElementAtIndex(1);
4834         final double biasZ = ba.getElementAtIndex(2);
4835 
4836         final Matrix ma = generateMaCommonAxis();
4837         final double sx = ma.getElementAt(0, 0);
4838         final double sy = ma.getElementAt(1, 1);
4839         final double sz = ma.getElementAt(2, 2);
4840         final double mxy = ma.getElementAt(0, 1);
4841         final double mxz = ma.getElementAt(0, 2);
4842         final double myx = ma.getElementAt(1, 0);
4843         final double myz = ma.getElementAt(1, 2);
4844         final double mzx = ma.getElementAt(2, 0);
4845         final double mzy = ma.getElementAt(2, 1);
4846 
4847         final KnownPositionAccelerometerCalibrator calibrator =
4848                 new KnownPositionAccelerometerCalibrator(measurements,
4849                         true, biasX, biasY, biasZ,
4850                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4851 
4852         // check default values
4853         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4854         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4855         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4856         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4857         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4858         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4859         final Acceleration bx2 = new Acceleration(0.0,
4860                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4861         calibrator.getInitialBiasXAsAcceleration(bx2);
4862         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4863         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4864         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4865         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4866         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4867         final Acceleration by2 = new Acceleration(0.0,
4868                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4869         calibrator.getInitialBiasYAsAcceleration(by2);
4870         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4871         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4872         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4873         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4874         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4875         final Acceleration bz2 = new Acceleration(0.0,
4876                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4877         calibrator.getInitialBiasZAsAcceleration(bz2);
4878         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4879         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4880         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4881         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4882         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4883         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4884         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4885         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4886         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4887         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4888         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4889         final double[] bias1 = calibrator.getInitialBias();
4890         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
4891         final double[] bias2 = new double[3];
4892         calibrator.getInitialBias(bias2);
4893         assertArrayEquals(bias1, bias2, 0.0);
4894         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4895         assertEquals(b1, ba);
4896         final Matrix b2 = new Matrix(3, 1);
4897         calibrator.getInitialBiasAsMatrix(b2);
4898         assertEquals(b1, b2);
4899         final Matrix ma1 = new Matrix(3, 3);
4900         ma1.setSubmatrix(0, 0,
4901                 2, 2,
4902                 new double[]{ sx, myx, mzx,
4903                         mxy, sy, mzy,
4904                         mxz, myz, sz});
4905         assertEquals(calibrator.getInitialMa(), ma1);
4906         final Matrix ma2 = new Matrix(3, 3);
4907         calibrator.getInitialMa(ma2);
4908         assertEquals(ma1, ma2);
4909         assertNull(calibrator.getEcefPosition());
4910         assertNull(calibrator.getNedPosition());
4911         assertFalse(calibrator.getNedPosition(null));
4912         assertSame(calibrator.getMeasurements(), measurements);
4913         assertTrue(calibrator.isCommonAxisUsed());
4914         assertSame(calibrator.getListener(), this);
4915         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4916         assertFalse(calibrator.isReady());
4917         assertFalse(calibrator.isRunning());
4918         assertNull(calibrator.getEstimatedBiases());
4919         assertFalse(calibrator.getEstimatedBiases(null));
4920         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4921         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4922         assertNull(calibrator.getEstimatedBiasFx());
4923         assertNull(calibrator.getEstimatedBiasFy());
4924         assertNull(calibrator.getEstimatedBiasFz());
4925         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4926         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4927         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4928         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4929         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4930         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4931         assertNull(calibrator.getEstimatedMa());
4932         assertNull(calibrator.getEstimatedSx());
4933         assertNull(calibrator.getEstimatedSy());
4934         assertNull(calibrator.getEstimatedSz());
4935         assertNull(calibrator.getEstimatedMxy());
4936         assertNull(calibrator.getEstimatedMxz());
4937         assertNull(calibrator.getEstimatedMyx());
4938         assertNull(calibrator.getEstimatedMyz());
4939         assertNull(calibrator.getEstimatedMzx());
4940         assertNull(calibrator.getEstimatedMzy());
4941         assertNull(calibrator.getEstimatedCovariance());
4942         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4943     }
4944 
4945     @Test
4946     public void testConstructor47() throws WrongSizeException {
4947         final Matrix ba = generateBa();
4948         final double biasX = ba.getElementAtIndex(0);
4949         final double biasY = ba.getElementAtIndex(1);
4950         final double biasZ = ba.getElementAtIndex(2);
4951 
4952         final Matrix ma = generateMaCommonAxis();
4953         final double sx = ma.getElementAt(0, 0);
4954         final double sy = ma.getElementAt(1, 1);
4955         final double sz = ma.getElementAt(2, 2);
4956         final double mxy = ma.getElementAt(0, 1);
4957         final double mxz = ma.getElementAt(0, 2);
4958         final double myx = ma.getElementAt(1, 0);
4959         final double myz = ma.getElementAt(1, 2);
4960         final double mzx = ma.getElementAt(2, 0);
4961         final double mzy = ma.getElementAt(2, 1);
4962 
4963         final Acceleration bx = new Acceleration(biasX,
4964                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4965         final Acceleration by = new Acceleration(biasY,
4966                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4967         final Acceleration bz = new Acceleration(biasZ,
4968                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4969 
4970         final KnownPositionAccelerometerCalibrator calibrator =
4971                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
4972                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4973 
4974         // check default values
4975         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4976         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4977         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4978         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4979         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4980         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4981         final Acceleration bx2 = new Acceleration(0.0,
4982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4983         calibrator.getInitialBiasXAsAcceleration(bx2);
4984         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4985         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4986         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4987         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4988         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4989         final Acceleration by2 = new Acceleration(0.0,
4990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4991         calibrator.getInitialBiasYAsAcceleration(by2);
4992         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4993         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4994         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4995         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4996         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4997         final Acceleration bz2 = new Acceleration(0.0,
4998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4999         calibrator.getInitialBiasZAsAcceleration(bz2);
5000         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5001         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5002         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5003         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5004         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5005         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5006         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5007         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5008         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5009         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5010         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5011         final double[] bias1 = calibrator.getInitialBias();
5012         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5013         final double[] bias2 = new double[3];
5014         calibrator.getInitialBias(bias2);
5015         assertArrayEquals(bias1, bias2, 0.0);
5016         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5017         assertEquals(b1, ba);
5018         final Matrix b2 = new Matrix(3, 1);
5019         calibrator.getInitialBiasAsMatrix(b2);
5020         assertEquals(b1, b2);
5021         final Matrix ma1 = new Matrix(3, 3);
5022         ma1.setSubmatrix(0, 0,
5023                 2, 2,
5024                 new double[]{ sx, myx, mzx,
5025                         mxy, sy, mzy,
5026                         mxz, myz, sz});
5027         assertEquals(calibrator.getInitialMa(), ma1);
5028         final Matrix ma2 = new Matrix(3, 3);
5029         calibrator.getInitialMa(ma2);
5030         assertEquals(ma1, ma2);
5031         assertNull(calibrator.getEcefPosition());
5032         assertNull(calibrator.getNedPosition());
5033         assertFalse(calibrator.getNedPosition(null));
5034         assertNull(calibrator.getMeasurements());
5035         assertFalse(calibrator.isCommonAxisUsed());
5036         assertNull(calibrator.getListener());
5037         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5038         assertFalse(calibrator.isReady());
5039         assertFalse(calibrator.isRunning());
5040         assertNull(calibrator.getEstimatedBiases());
5041         assertFalse(calibrator.getEstimatedBiases(null));
5042         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5043         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5044         assertNull(calibrator.getEstimatedBiasFx());
5045         assertNull(calibrator.getEstimatedBiasFy());
5046         assertNull(calibrator.getEstimatedBiasFz());
5047         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5048         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5049         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5050         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5051         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5052         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5053         assertNull(calibrator.getEstimatedMa());
5054         assertNull(calibrator.getEstimatedSx());
5055         assertNull(calibrator.getEstimatedSy());
5056         assertNull(calibrator.getEstimatedSz());
5057         assertNull(calibrator.getEstimatedMxy());
5058         assertNull(calibrator.getEstimatedMxz());
5059         assertNull(calibrator.getEstimatedMyx());
5060         assertNull(calibrator.getEstimatedMyz());
5061         assertNull(calibrator.getEstimatedMzx());
5062         assertNull(calibrator.getEstimatedMzy());
5063         assertNull(calibrator.getEstimatedCovariance());
5064         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5065     }
5066 
5067     @Test
5068     public void testConstructor48() throws WrongSizeException {
5069         final Matrix ba = generateBa();
5070         final double biasX = ba.getElementAtIndex(0);
5071         final double biasY = ba.getElementAtIndex(1);
5072         final double biasZ = ba.getElementAtIndex(2);
5073 
5074         final Matrix ma = generateMaCommonAxis();
5075         final double sx = ma.getElementAt(0, 0);
5076         final double sy = ma.getElementAt(1, 1);
5077         final double sz = ma.getElementAt(2, 2);
5078         final double mxy = ma.getElementAt(0, 1);
5079         final double mxz = ma.getElementAt(0, 2);
5080         final double myx = ma.getElementAt(1, 0);
5081         final double myz = ma.getElementAt(1, 2);
5082         final double mzx = ma.getElementAt(2, 0);
5083         final double mzy = ma.getElementAt(2, 1);
5084 
5085         final Acceleration bx = new Acceleration(biasX,
5086                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5087         final Acceleration by = new Acceleration(biasY,
5088                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5089         final Acceleration bz = new Acceleration(biasZ,
5090                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5091 
5092         final KnownPositionAccelerometerCalibrator calibrator =
5093                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5094                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5095 
5096         // check default values
5097         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5098         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5099         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5100         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5101         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5102         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5103         final Acceleration bx2 = new Acceleration(0.0,
5104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5105         calibrator.getInitialBiasXAsAcceleration(bx2);
5106         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5107         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5108         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5109         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5110         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5111         final Acceleration by2 = new Acceleration(0.0,
5112                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5113         calibrator.getInitialBiasYAsAcceleration(by2);
5114         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5115         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5116         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5117         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5118         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5119         final Acceleration bz2 = new Acceleration(0.0,
5120                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5121         calibrator.getInitialBiasZAsAcceleration(bz2);
5122         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5123         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5124         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5125         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5126         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5127         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5128         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5129         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5130         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5131         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5132         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5133         final double[] bias1 = calibrator.getInitialBias();
5134         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5135         final double[] bias2 = new double[3];
5136         calibrator.getInitialBias(bias2);
5137         assertArrayEquals(bias1, bias2, 0.0);
5138         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5139         assertEquals(b1, ba);
5140         final Matrix b2 = new Matrix(3, 1);
5141         calibrator.getInitialBiasAsMatrix(b2);
5142         assertEquals(b1, b2);
5143         final Matrix ma1 = new Matrix(3, 3);
5144         ma1.setSubmatrix(0, 0,
5145                 2, 2,
5146                 new double[]{ sx, myx, mzx,
5147                         mxy, sy, mzy,
5148                         mxz, myz, sz});
5149         assertEquals(calibrator.getInitialMa(), ma1);
5150         final Matrix ma2 = new Matrix(3, 3);
5151         calibrator.getInitialMa(ma2);
5152         assertEquals(ma1, ma2);
5153         assertNull(calibrator.getEcefPosition());
5154         assertNull(calibrator.getNedPosition());
5155         assertFalse(calibrator.getNedPosition(null));
5156         assertNull(calibrator.getMeasurements());
5157         assertFalse(calibrator.isCommonAxisUsed());
5158         assertSame(calibrator.getListener(), this);
5159         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5160         assertFalse(calibrator.isReady());
5161         assertFalse(calibrator.isRunning());
5162         assertNull(calibrator.getEstimatedBiases());
5163         assertFalse(calibrator.getEstimatedBiases(null));
5164         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5165         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5166         assertNull(calibrator.getEstimatedBiasFx());
5167         assertNull(calibrator.getEstimatedBiasFy());
5168         assertNull(calibrator.getEstimatedBiasFz());
5169         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5170         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5171         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5172         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5173         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5174         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5175         assertNull(calibrator.getEstimatedMa());
5176         assertNull(calibrator.getEstimatedSx());
5177         assertNull(calibrator.getEstimatedSy());
5178         assertNull(calibrator.getEstimatedSz());
5179         assertNull(calibrator.getEstimatedMxy());
5180         assertNull(calibrator.getEstimatedMxz());
5181         assertNull(calibrator.getEstimatedMyx());
5182         assertNull(calibrator.getEstimatedMyz());
5183         assertNull(calibrator.getEstimatedMzx());
5184         assertNull(calibrator.getEstimatedMzy());
5185         assertNull(calibrator.getEstimatedCovariance());
5186         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5187     }
5188 
5189     @Test
5190     public void testConstructor49() throws WrongSizeException {
5191 
5192         final Collection<StandardDeviationBodyKinematics> measurements =
5193                 Collections.emptyList();
5194 
5195         final Matrix ba = generateBa();
5196         final double biasX = ba.getElementAtIndex(0);
5197         final double biasY = ba.getElementAtIndex(1);
5198         final double biasZ = ba.getElementAtIndex(2);
5199 
5200         final Matrix ma = generateMaCommonAxis();
5201         final double sx = ma.getElementAt(0, 0);
5202         final double sy = ma.getElementAt(1, 1);
5203         final double sz = ma.getElementAt(2, 2);
5204         final double mxy = ma.getElementAt(0, 1);
5205         final double mxz = ma.getElementAt(0, 2);
5206         final double myx = ma.getElementAt(1, 0);
5207         final double myz = ma.getElementAt(1, 2);
5208         final double mzx = ma.getElementAt(2, 0);
5209         final double mzy = ma.getElementAt(2, 1);
5210 
5211         final Acceleration bx = new Acceleration(biasX,
5212                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5213         final Acceleration by = new Acceleration(biasY,
5214                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5215         final Acceleration bz = new Acceleration(biasZ,
5216                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5217 
5218         final KnownPositionAccelerometerCalibrator calibrator =
5219                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5220                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5221 
5222         // check default values
5223         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5224         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5225         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5226         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5227         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5228         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5229         final Acceleration bx2 = new Acceleration(0.0,
5230                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5231         calibrator.getInitialBiasXAsAcceleration(bx2);
5232         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5233         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5234         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5235         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5236         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5237         final Acceleration by2 = new Acceleration(0.0,
5238                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5239         calibrator.getInitialBiasYAsAcceleration(by2);
5240         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5241         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5242         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5243         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5244         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5245         final Acceleration bz2 = new Acceleration(0.0,
5246                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5247         calibrator.getInitialBiasZAsAcceleration(bz2);
5248         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5249         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5250         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5251         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5252         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5253         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5254         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5255         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5256         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5257         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5258         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5259         final double[] bias1 = calibrator.getInitialBias();
5260         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5261         final double[] bias2 = new double[3];
5262         calibrator.getInitialBias(bias2);
5263         assertArrayEquals(bias1, bias2, 0.0);
5264         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5265         assertEquals(b1, ba);
5266         final Matrix b2 = new Matrix(3, 1);
5267         calibrator.getInitialBiasAsMatrix(b2);
5268         assertEquals(b1, b2);
5269         final Matrix ma1 = new Matrix(3, 3);
5270         ma1.setSubmatrix(0, 0,
5271                 2, 2,
5272                 new double[]{ sx, myx, mzx,
5273                         mxy, sy, mzy,
5274                         mxz, myz, sz});
5275         assertEquals(calibrator.getInitialMa(), ma1);
5276         final Matrix ma2 = new Matrix(3, 3);
5277         calibrator.getInitialMa(ma2);
5278         assertEquals(ma1, ma2);
5279         assertNull(calibrator.getEcefPosition());
5280         assertNull(calibrator.getNedPosition());
5281         assertFalse(calibrator.getNedPosition(null));
5282         assertSame(calibrator.getMeasurements(), measurements);
5283         assertFalse(calibrator.isCommonAxisUsed());
5284         assertNull(calibrator.getListener());
5285         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5286         assertFalse(calibrator.isReady());
5287         assertFalse(calibrator.isRunning());
5288         assertNull(calibrator.getEstimatedBiases());
5289         assertFalse(calibrator.getEstimatedBiases(null));
5290         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5291         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5292         assertNull(calibrator.getEstimatedBiasFx());
5293         assertNull(calibrator.getEstimatedBiasFy());
5294         assertNull(calibrator.getEstimatedBiasFz());
5295         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5296         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5297         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5298         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5299         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5300         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5301         assertNull(calibrator.getEstimatedMa());
5302         assertNull(calibrator.getEstimatedSx());
5303         assertNull(calibrator.getEstimatedSy());
5304         assertNull(calibrator.getEstimatedSz());
5305         assertNull(calibrator.getEstimatedMxy());
5306         assertNull(calibrator.getEstimatedMxz());
5307         assertNull(calibrator.getEstimatedMyx());
5308         assertNull(calibrator.getEstimatedMyz());
5309         assertNull(calibrator.getEstimatedMzx());
5310         assertNull(calibrator.getEstimatedMzy());
5311         assertNull(calibrator.getEstimatedCovariance());
5312         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5313     }
5314 
5315     @Test
5316     public void testConstructor50() throws WrongSizeException {
5317 
5318         final Collection<StandardDeviationBodyKinematics> measurements =
5319                 Collections.emptyList();
5320 
5321         final Matrix ba = generateBa();
5322         final double biasX = ba.getElementAtIndex(0);
5323         final double biasY = ba.getElementAtIndex(1);
5324         final double biasZ = ba.getElementAtIndex(2);
5325 
5326         final Matrix ma = generateMaCommonAxis();
5327         final double sx = ma.getElementAt(0, 0);
5328         final double sy = ma.getElementAt(1, 1);
5329         final double sz = ma.getElementAt(2, 2);
5330         final double mxy = ma.getElementAt(0, 1);
5331         final double mxz = ma.getElementAt(0, 2);
5332         final double myx = ma.getElementAt(1, 0);
5333         final double myz = ma.getElementAt(1, 2);
5334         final double mzx = ma.getElementAt(2, 0);
5335         final double mzy = ma.getElementAt(2, 1);
5336 
5337         final Acceleration bx = new Acceleration(biasX,
5338                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5339         final Acceleration by = new Acceleration(biasY,
5340                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5341         final Acceleration bz = new Acceleration(biasZ,
5342                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5343 
5344         final KnownPositionAccelerometerCalibrator calibrator =
5345                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5346                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5347 
5348         // check default values
5349         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5350         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5351         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5352         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5353         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5354         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5355         final Acceleration bx2 = new Acceleration(0.0,
5356                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5357         calibrator.getInitialBiasXAsAcceleration(bx2);
5358         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5359         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5360         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5361         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5362         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5363         final Acceleration by2 = new Acceleration(0.0,
5364                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5365         calibrator.getInitialBiasYAsAcceleration(by2);
5366         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5367         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5368         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5369         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5370         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5371         final Acceleration bz2 = new Acceleration(0.0,
5372                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5373         calibrator.getInitialBiasZAsAcceleration(bz2);
5374         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5375         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5376         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5377         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5378         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5379         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5380         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5381         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5382         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5383         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5384         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5385         final double[] bias1 = calibrator.getInitialBias();
5386         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5387         final double[] bias2 = new double[3];
5388         calibrator.getInitialBias(bias2);
5389         assertArrayEquals(bias1, bias2, 0.0);
5390         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5391         assertEquals(b1, ba);
5392         final Matrix b2 = new Matrix(3, 1);
5393         calibrator.getInitialBiasAsMatrix(b2);
5394         assertEquals(b1, b2);
5395         final Matrix ma1 = new Matrix(3, 3);
5396         ma1.setSubmatrix(0, 0,
5397                 2, 2,
5398                 new double[]{ sx, myx, mzx,
5399                         mxy, sy, mzy,
5400                         mxz, myz, sz});
5401         assertEquals(calibrator.getInitialMa(), ma1);
5402         final Matrix ma2 = new Matrix(3, 3);
5403         calibrator.getInitialMa(ma2);
5404         assertEquals(ma1, ma2);
5405         assertNull(calibrator.getEcefPosition());
5406         assertNull(calibrator.getNedPosition());
5407         assertFalse(calibrator.getNedPosition(null));
5408         assertSame(calibrator.getMeasurements(), measurements);
5409         assertFalse(calibrator.isCommonAxisUsed());
5410         assertSame(calibrator.getListener(), this);
5411         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5412         assertFalse(calibrator.isReady());
5413         assertFalse(calibrator.isRunning());
5414         assertNull(calibrator.getEstimatedBiases());
5415         assertFalse(calibrator.getEstimatedBiases(null));
5416         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5417         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5418         assertNull(calibrator.getEstimatedBiasFx());
5419         assertNull(calibrator.getEstimatedBiasFy());
5420         assertNull(calibrator.getEstimatedBiasFz());
5421         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5422         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5423         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5424         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5425         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5426         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5427         assertNull(calibrator.getEstimatedMa());
5428         assertNull(calibrator.getEstimatedSx());
5429         assertNull(calibrator.getEstimatedSy());
5430         assertNull(calibrator.getEstimatedSz());
5431         assertNull(calibrator.getEstimatedMxy());
5432         assertNull(calibrator.getEstimatedMxz());
5433         assertNull(calibrator.getEstimatedMyx());
5434         assertNull(calibrator.getEstimatedMyz());
5435         assertNull(calibrator.getEstimatedMzx());
5436         assertNull(calibrator.getEstimatedMzy());
5437         assertNull(calibrator.getEstimatedCovariance());
5438         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5439     }
5440 
5441     @Test
5442     public void testConstructor51() throws WrongSizeException {
5443         final Matrix ba = generateBa();
5444         final double biasX = ba.getElementAtIndex(0);
5445         final double biasY = ba.getElementAtIndex(1);
5446         final double biasZ = ba.getElementAtIndex(2);
5447 
5448         final Matrix ma = generateMaCommonAxis();
5449         final double sx = ma.getElementAt(0, 0);
5450         final double sy = ma.getElementAt(1, 1);
5451         final double sz = ma.getElementAt(2, 2);
5452         final double mxy = ma.getElementAt(0, 1);
5453         final double mxz = ma.getElementAt(0, 2);
5454         final double myx = ma.getElementAt(1, 0);
5455         final double myz = ma.getElementAt(1, 2);
5456         final double mzx = ma.getElementAt(2, 0);
5457         final double mzy = ma.getElementAt(2, 1);
5458 
5459         final Acceleration bx = new Acceleration(biasX,
5460                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5461         final Acceleration by = new Acceleration(biasY,
5462                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5463         final Acceleration bz = new Acceleration(biasZ,
5464                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5465 
5466         final KnownPositionAccelerometerCalibrator calibrator =
5467                 new KnownPositionAccelerometerCalibrator(true,
5468                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5469 
5470         // check default values
5471         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5472         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5473         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5474         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5475         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5476         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5477         final Acceleration bx2 = new Acceleration(0.0,
5478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5479         calibrator.getInitialBiasXAsAcceleration(bx2);
5480         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5481         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5482         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5483         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5484         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5485         final Acceleration by2 = new Acceleration(0.0,
5486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5487         calibrator.getInitialBiasYAsAcceleration(by2);
5488         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5489         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5491         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5492         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5493         final Acceleration bz2 = new Acceleration(0.0,
5494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5495         calibrator.getInitialBiasZAsAcceleration(bz2);
5496         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5497         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5498         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5499         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5500         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5501         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5502         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5503         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5504         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5505         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5506         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5507         final double[] bias1 = calibrator.getInitialBias();
5508         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5509         final double[] bias2 = new double[3];
5510         calibrator.getInitialBias(bias2);
5511         assertArrayEquals(bias1, bias2, 0.0);
5512         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5513         assertEquals(b1, ba);
5514         final Matrix b2 = new Matrix(3, 1);
5515         calibrator.getInitialBiasAsMatrix(b2);
5516         assertEquals(b1, b2);
5517         final Matrix ma1 = new Matrix(3, 3);
5518         ma1.setSubmatrix(0, 0,
5519                 2, 2,
5520                 new double[]{ sx, myx, mzx,
5521                         mxy, sy, mzy,
5522                         mxz, myz, sz});
5523         assertEquals(calibrator.getInitialMa(), ma1);
5524         final Matrix ma2 = new Matrix(3, 3);
5525         calibrator.getInitialMa(ma2);
5526         assertEquals(ma1, ma2);
5527         assertNull(calibrator.getEcefPosition());
5528         assertNull(calibrator.getNedPosition());
5529         assertFalse(calibrator.getNedPosition(null));
5530         assertNull(calibrator.getMeasurements());
5531         assertTrue(calibrator.isCommonAxisUsed());
5532         assertNull(calibrator.getListener());
5533         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5534         assertFalse(calibrator.isReady());
5535         assertFalse(calibrator.isRunning());
5536         assertNull(calibrator.getEstimatedBiases());
5537         assertFalse(calibrator.getEstimatedBiases(null));
5538         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5539         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5540         assertNull(calibrator.getEstimatedBiasFx());
5541         assertNull(calibrator.getEstimatedBiasFy());
5542         assertNull(calibrator.getEstimatedBiasFz());
5543         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5544         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5545         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5546         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5547         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5548         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5549         assertNull(calibrator.getEstimatedMa());
5550         assertNull(calibrator.getEstimatedSx());
5551         assertNull(calibrator.getEstimatedSy());
5552         assertNull(calibrator.getEstimatedSz());
5553         assertNull(calibrator.getEstimatedMxy());
5554         assertNull(calibrator.getEstimatedMxz());
5555         assertNull(calibrator.getEstimatedMyx());
5556         assertNull(calibrator.getEstimatedMyz());
5557         assertNull(calibrator.getEstimatedMzx());
5558         assertNull(calibrator.getEstimatedMzy());
5559         assertNull(calibrator.getEstimatedCovariance());
5560         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5561     }
5562 
5563     @Test
5564     public void testConstructor52() throws WrongSizeException {
5565         final Matrix ba = generateBa();
5566         final double biasX = ba.getElementAtIndex(0);
5567         final double biasY = ba.getElementAtIndex(1);
5568         final double biasZ = ba.getElementAtIndex(2);
5569 
5570         final Matrix ma = generateMaCommonAxis();
5571         final double sx = ma.getElementAt(0, 0);
5572         final double sy = ma.getElementAt(1, 1);
5573         final double sz = ma.getElementAt(2, 2);
5574         final double mxy = ma.getElementAt(0, 1);
5575         final double mxz = ma.getElementAt(0, 2);
5576         final double myx = ma.getElementAt(1, 0);
5577         final double myz = ma.getElementAt(1, 2);
5578         final double mzx = ma.getElementAt(2, 0);
5579         final double mzy = ma.getElementAt(2, 1);
5580 
5581         final Acceleration bx = new Acceleration(biasX,
5582                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5583         final Acceleration by = new Acceleration(biasY,
5584                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5585         final Acceleration bz = new Acceleration(biasZ,
5586                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5587 
5588         final KnownPositionAccelerometerCalibrator calibrator =
5589                 new KnownPositionAccelerometerCalibrator(true,
5590                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5591                         this);
5592 
5593         // check default values
5594         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5595         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5596         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5597         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5598         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5599         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5600         final Acceleration bx2 = new Acceleration(0.0,
5601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5602         calibrator.getInitialBiasXAsAcceleration(bx2);
5603         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5604         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5605         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5606         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5607         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5608         final Acceleration by2 = new Acceleration(0.0,
5609                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5610         calibrator.getInitialBiasYAsAcceleration(by2);
5611         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5612         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5613         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5614         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5615         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5616         final Acceleration bz2 = new Acceleration(0.0,
5617                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5618         calibrator.getInitialBiasZAsAcceleration(bz2);
5619         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5620         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5621         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5622         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5623         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5624         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5625         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5626         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5627         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5628         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5629         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5630         final double[] bias1 = calibrator.getInitialBias();
5631         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5632         final double[] bias2 = new double[3];
5633         calibrator.getInitialBias(bias2);
5634         assertArrayEquals(bias1, bias2, 0.0);
5635         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5636         assertEquals(b1, ba);
5637         final Matrix b2 = new Matrix(3, 1);
5638         calibrator.getInitialBiasAsMatrix(b2);
5639         assertEquals(b1, b2);
5640         final Matrix ma1 = new Matrix(3, 3);
5641         ma1.setSubmatrix(0, 0,
5642                 2, 2,
5643                 new double[]{ sx, myx, mzx,
5644                         mxy, sy, mzy,
5645                         mxz, myz, sz});
5646         assertEquals(calibrator.getInitialMa(), ma1);
5647         final Matrix ma2 = new Matrix(3, 3);
5648         calibrator.getInitialMa(ma2);
5649         assertEquals(ma1, ma2);
5650         assertNull(calibrator.getEcefPosition());
5651         assertNull(calibrator.getNedPosition());
5652         assertFalse(calibrator.getNedPosition(null));
5653         assertNull(calibrator.getMeasurements());
5654         assertTrue(calibrator.isCommonAxisUsed());
5655         assertSame(calibrator.getListener(), this);
5656         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5657         assertFalse(calibrator.isReady());
5658         assertFalse(calibrator.isRunning());
5659         assertNull(calibrator.getEstimatedBiases());
5660         assertFalse(calibrator.getEstimatedBiases(null));
5661         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5662         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5663         assertNull(calibrator.getEstimatedBiasFx());
5664         assertNull(calibrator.getEstimatedBiasFy());
5665         assertNull(calibrator.getEstimatedBiasFz());
5666         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5667         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5668         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5669         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5670         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5671         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5672         assertNull(calibrator.getEstimatedMa());
5673         assertNull(calibrator.getEstimatedSx());
5674         assertNull(calibrator.getEstimatedSy());
5675         assertNull(calibrator.getEstimatedSz());
5676         assertNull(calibrator.getEstimatedMxy());
5677         assertNull(calibrator.getEstimatedMxz());
5678         assertNull(calibrator.getEstimatedMyx());
5679         assertNull(calibrator.getEstimatedMyz());
5680         assertNull(calibrator.getEstimatedMzx());
5681         assertNull(calibrator.getEstimatedMzy());
5682         assertNull(calibrator.getEstimatedCovariance());
5683         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5684     }
5685 
5686     @Test
5687     public void testConstructor53() throws WrongSizeException {
5688         final Collection<StandardDeviationBodyKinematics> measurements =
5689                 Collections.emptyList();
5690 
5691         final Matrix ba = generateBa();
5692         final double biasX = ba.getElementAtIndex(0);
5693         final double biasY = ba.getElementAtIndex(1);
5694         final double biasZ = ba.getElementAtIndex(2);
5695 
5696         final Matrix ma = generateMaCommonAxis();
5697         final double sx = ma.getElementAt(0, 0);
5698         final double sy = ma.getElementAt(1, 1);
5699         final double sz = ma.getElementAt(2, 2);
5700         final double mxy = ma.getElementAt(0, 1);
5701         final double mxz = ma.getElementAt(0, 2);
5702         final double myx = ma.getElementAt(1, 0);
5703         final double myz = ma.getElementAt(1, 2);
5704         final double mzx = ma.getElementAt(2, 0);
5705         final double mzy = ma.getElementAt(2, 1);
5706 
5707         final Acceleration bx = new Acceleration(biasX,
5708                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5709         final Acceleration by = new Acceleration(biasY,
5710                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5711         final Acceleration bz = new Acceleration(biasZ,
5712                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5713 
5714         final KnownPositionAccelerometerCalibrator calibrator =
5715                 new KnownPositionAccelerometerCalibrator(measurements,
5716                         true, bx, by, bz, sx, sy, sz,
5717                         mxy, mxz, myx, myz, mzx, mzy);
5718 
5719         // check default values
5720         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5721         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5722         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5723         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5724         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5725         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5726         final Acceleration bx2 = new Acceleration(0.0,
5727                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5728         calibrator.getInitialBiasXAsAcceleration(bx2);
5729         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5730         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5731         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5732         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5733         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5734         final Acceleration by2 = new Acceleration(0.0,
5735                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5736         calibrator.getInitialBiasYAsAcceleration(by2);
5737         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5738         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5739         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5740         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5741         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5742         final Acceleration bz2 = new Acceleration(0.0,
5743                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5744         calibrator.getInitialBiasZAsAcceleration(bz2);
5745         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5746         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5747         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5748         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5749         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5750         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5751         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5752         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5753         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5754         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5755         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5756         final double[] bias1 = calibrator.getInitialBias();
5757         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5758         final double[] bias2 = new double[3];
5759         calibrator.getInitialBias(bias2);
5760         assertArrayEquals(bias1, bias2, 0.0);
5761         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5762         assertEquals(b1, ba);
5763         final Matrix b2 = new Matrix(3, 1);
5764         calibrator.getInitialBiasAsMatrix(b2);
5765         assertEquals(b1, b2);
5766         final Matrix ma1 = new Matrix(3, 3);
5767         ma1.setSubmatrix(0, 0,
5768                 2, 2,
5769                 new double[]{ sx, myx, mzx,
5770                         mxy, sy, mzy,
5771                         mxz, myz, sz});
5772         assertEquals(calibrator.getInitialMa(), ma1);
5773         final Matrix ma2 = new Matrix(3, 3);
5774         calibrator.getInitialMa(ma2);
5775         assertEquals(ma1, ma2);
5776         assertNull(calibrator.getEcefPosition());
5777         assertNull(calibrator.getNedPosition());
5778         assertFalse(calibrator.getNedPosition(null));
5779         assertSame(calibrator.getMeasurements(), measurements);
5780         assertTrue(calibrator.isCommonAxisUsed());
5781         assertNull(calibrator.getListener());
5782         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5783         assertFalse(calibrator.isReady());
5784         assertFalse(calibrator.isRunning());
5785         assertNull(calibrator.getEstimatedBiases());
5786         assertFalse(calibrator.getEstimatedBiases(null));
5787         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5788         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5789         assertNull(calibrator.getEstimatedBiasFx());
5790         assertNull(calibrator.getEstimatedBiasFy());
5791         assertNull(calibrator.getEstimatedBiasFz());
5792         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5793         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5794         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5795         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5796         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5797         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5798         assertNull(calibrator.getEstimatedMa());
5799         assertNull(calibrator.getEstimatedSx());
5800         assertNull(calibrator.getEstimatedSy());
5801         assertNull(calibrator.getEstimatedSz());
5802         assertNull(calibrator.getEstimatedMxy());
5803         assertNull(calibrator.getEstimatedMxz());
5804         assertNull(calibrator.getEstimatedMyx());
5805         assertNull(calibrator.getEstimatedMyz());
5806         assertNull(calibrator.getEstimatedMzx());
5807         assertNull(calibrator.getEstimatedMzy());
5808         assertNull(calibrator.getEstimatedCovariance());
5809         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5810     }
5811 
5812     @Test
5813     public void testConstructor54() throws WrongSizeException {
5814         final Collection<StandardDeviationBodyKinematics> measurements =
5815                 Collections.emptyList();
5816 
5817         final Matrix ba = generateBa();
5818         final double biasX = ba.getElementAtIndex(0);
5819         final double biasY = ba.getElementAtIndex(1);
5820         final double biasZ = ba.getElementAtIndex(2);
5821 
5822         final Matrix ma = generateMaCommonAxis();
5823         final double sx = ma.getElementAt(0, 0);
5824         final double sy = ma.getElementAt(1, 1);
5825         final double sz = ma.getElementAt(2, 2);
5826         final double mxy = ma.getElementAt(0, 1);
5827         final double mxz = ma.getElementAt(0, 2);
5828         final double myx = ma.getElementAt(1, 0);
5829         final double myz = ma.getElementAt(1, 2);
5830         final double mzx = ma.getElementAt(2, 0);
5831         final double mzy = ma.getElementAt(2, 1);
5832 
5833         final Acceleration bx = new Acceleration(biasX,
5834                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5835         final Acceleration by = new Acceleration(biasY,
5836                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5837         final Acceleration bz = new Acceleration(biasZ,
5838                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5839 
5840         final KnownPositionAccelerometerCalibrator calibrator =
5841                 new KnownPositionAccelerometerCalibrator(measurements,
5842                         true, bx, by, bz, sx, sy, sz,
5843                         mxy, mxz, myx, myz, mzx, mzy, this);
5844 
5845         // check default values
5846         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5847         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5848         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5849         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5850         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5851         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5852         final Acceleration bx2 = new Acceleration(0.0,
5853                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5854         calibrator.getInitialBiasXAsAcceleration(bx2);
5855         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5856         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5857         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5858         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5859         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5860         final Acceleration by2 = new Acceleration(0.0,
5861                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5862         calibrator.getInitialBiasYAsAcceleration(by2);
5863         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5864         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5865         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5866         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5867         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5868         final Acceleration bz2 = new Acceleration(0.0,
5869                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5870         calibrator.getInitialBiasZAsAcceleration(bz2);
5871         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5872         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5873         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5874         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5875         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5876         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5877         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5878         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5879         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5880         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5881         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5882         final double[] bias1 = calibrator.getInitialBias();
5883         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5884         final double[] bias2 = new double[3];
5885         calibrator.getInitialBias(bias2);
5886         assertArrayEquals(bias1, bias2, 0.0);
5887         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5888         assertEquals(b1, ba);
5889         final Matrix b2 = new Matrix(3, 1);
5890         calibrator.getInitialBiasAsMatrix(b2);
5891         assertEquals(b1, b2);
5892         final Matrix ma1 = new Matrix(3, 3);
5893         ma1.setSubmatrix(0, 0,
5894                 2, 2,
5895                 new double[]{ sx, myx, mzx,
5896                         mxy, sy, mzy,
5897                         mxz, myz, sz});
5898         assertEquals(calibrator.getInitialMa(), ma1);
5899         final Matrix ma2 = new Matrix(3, 3);
5900         calibrator.getInitialMa(ma2);
5901         assertEquals(ma1, ma2);
5902         assertNull(calibrator.getEcefPosition());
5903         assertNull(calibrator.getNedPosition());
5904         assertFalse(calibrator.getNedPosition(null));
5905         assertSame(calibrator.getMeasurements(), measurements);
5906         assertTrue(calibrator.isCommonAxisUsed());
5907         assertSame(calibrator.getListener(), this);
5908         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5909         assertFalse(calibrator.isReady());
5910         assertFalse(calibrator.isRunning());
5911         assertNull(calibrator.getEstimatedBiases());
5912         assertFalse(calibrator.getEstimatedBiases(null));
5913         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5914         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5915         assertNull(calibrator.getEstimatedBiasFx());
5916         assertNull(calibrator.getEstimatedBiasFy());
5917         assertNull(calibrator.getEstimatedBiasFz());
5918         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5919         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5920         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5921         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5922         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5923         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5924         assertNull(calibrator.getEstimatedMa());
5925         assertNull(calibrator.getEstimatedSx());
5926         assertNull(calibrator.getEstimatedSy());
5927         assertNull(calibrator.getEstimatedSz());
5928         assertNull(calibrator.getEstimatedMxy());
5929         assertNull(calibrator.getEstimatedMxz());
5930         assertNull(calibrator.getEstimatedMyx());
5931         assertNull(calibrator.getEstimatedMyz());
5932         assertNull(calibrator.getEstimatedMzx());
5933         assertNull(calibrator.getEstimatedMzy());
5934         assertNull(calibrator.getEstimatedCovariance());
5935         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5936     }
5937 
5938     @Test
5939     public void testConstructor55() throws WrongSizeException {
5940         final Matrix ba = generateBa();
5941         final double[] bias = ba.getBuffer();
5942         final double biasX = ba.getElementAtIndex(0);
5943         final double biasY = ba.getElementAtIndex(1);
5944         final double biasZ = ba.getElementAtIndex(2);
5945 
5946         KnownPositionAccelerometerCalibrator calibrator =
5947                 new KnownPositionAccelerometerCalibrator(bias);
5948 
5949         // check default values
5950         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5951         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5952         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5953         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5954         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5955         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5956         final Acceleration bx2 = new Acceleration(0.0,
5957                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5958         calibrator.getInitialBiasXAsAcceleration(bx2);
5959         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5960         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5961         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5962         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5963         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5964         final Acceleration by2 = new Acceleration(0.0,
5965                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5966         calibrator.getInitialBiasYAsAcceleration(by2);
5967         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5968         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5969         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5970         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5971         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5972         final Acceleration bz2 = new Acceleration(0.0,
5973                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5974         calibrator.getInitialBiasZAsAcceleration(bz2);
5975         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5976         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5977         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5978         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5979         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5980         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5981         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5982         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5983         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5984         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5985         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5986         final double[] bias1 = calibrator.getInitialBias();
5987         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
5988         final double[] bias2 = new double[3];
5989         calibrator.getInitialBias(bias2);
5990         assertArrayEquals(bias1, bias2, 0.0);
5991         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5992         assertEquals(b1, ba);
5993         final Matrix b2 = new Matrix(3, 1);
5994         calibrator.getInitialBiasAsMatrix(b2);
5995         assertEquals(b1, b2);
5996         final Matrix ma1 = calibrator.getInitialMa();
5997         assertEquals(ma1, new Matrix(3, 3));
5998         final Matrix ma2 = new Matrix(3, 3);
5999         calibrator.getInitialMa(ma2);
6000         assertEquals(ma1, ma2);
6001         assertNull(calibrator.getEcefPosition());
6002         assertNull(calibrator.getNedPosition());
6003         assertFalse(calibrator.getNedPosition(null));
6004         assertNull(calibrator.getMeasurements());
6005         assertFalse(calibrator.isCommonAxisUsed());
6006         assertNull(calibrator.getListener());
6007         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6008         assertFalse(calibrator.isReady());
6009         assertFalse(calibrator.isRunning());
6010         assertNull(calibrator.getEstimatedBiases());
6011         assertFalse(calibrator.getEstimatedBiases(null));
6012         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6013         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6014         assertNull(calibrator.getEstimatedBiasFx());
6015         assertNull(calibrator.getEstimatedBiasFy());
6016         assertNull(calibrator.getEstimatedBiasFz());
6017         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6018         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6019         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6020         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6021         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6022         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6023         assertNull(calibrator.getEstimatedMa());
6024         assertNull(calibrator.getEstimatedSx());
6025         assertNull(calibrator.getEstimatedSy());
6026         assertNull(calibrator.getEstimatedSz());
6027         assertNull(calibrator.getEstimatedMxy());
6028         assertNull(calibrator.getEstimatedMxz());
6029         assertNull(calibrator.getEstimatedMyx());
6030         assertNull(calibrator.getEstimatedMyz());
6031         assertNull(calibrator.getEstimatedMzx());
6032         assertNull(calibrator.getEstimatedMzy());
6033         assertNull(calibrator.getEstimatedCovariance());
6034         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6035 
6036         // Force IllegalArgumentException
6037         calibrator = null;
6038         try {
6039             calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6040             fail("IllegalArgumentException expected but not thrown");
6041         } catch (final IllegalArgumentException ignore) {
6042         }
6043         assertNull(calibrator);
6044     }
6045 
6046     @Test
6047     public void testConstructor56() throws WrongSizeException {
6048         final Matrix ba = generateBa();
6049         final double[] bias = ba.getBuffer();
6050         final double biasX = ba.getElementAtIndex(0);
6051         final double biasY = ba.getElementAtIndex(1);
6052         final double biasZ = ba.getElementAtIndex(2);
6053 
6054         KnownPositionAccelerometerCalibrator calibrator =
6055                 new KnownPositionAccelerometerCalibrator(bias, this);
6056 
6057         // check default values
6058         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6059         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6060         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6061         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6062         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6063         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6064         final Acceleration bx2 = new Acceleration(0.0,
6065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6066         calibrator.getInitialBiasXAsAcceleration(bx2);
6067         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6068         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6069         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6070         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6071         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6072         final Acceleration by2 = new Acceleration(0.0,
6073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6074         calibrator.getInitialBiasYAsAcceleration(by2);
6075         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6076         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6077         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6078         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6079         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6080         final Acceleration bz2 = new Acceleration(0.0,
6081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6082         calibrator.getInitialBiasZAsAcceleration(bz2);
6083         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6084         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6085         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6086         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6087         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6088         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6089         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6090         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6091         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6092         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6093         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6094         final double[] bias1 = calibrator.getInitialBias();
6095         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6096         final double[] bias2 = new double[3];
6097         calibrator.getInitialBias(bias2);
6098         assertArrayEquals(bias1, bias2, 0.0);
6099         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6100         assertEquals(b1, ba);
6101         final Matrix b2 = new Matrix(3, 1);
6102         calibrator.getInitialBiasAsMatrix(b2);
6103         assertEquals(b1, b2);
6104         final Matrix ma1 = calibrator.getInitialMa();
6105         assertEquals(ma1, new Matrix(3, 3));
6106         final Matrix ma2 = new Matrix(3, 3);
6107         calibrator.getInitialMa(ma2);
6108         assertEquals(ma1, ma2);
6109         assertNull(calibrator.getEcefPosition());
6110         assertNull(calibrator.getNedPosition());
6111         assertFalse(calibrator.getNedPosition(null));
6112         assertNull(calibrator.getMeasurements());
6113         assertFalse(calibrator.isCommonAxisUsed());
6114         assertSame(calibrator.getListener(), this);
6115         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6116         assertFalse(calibrator.isReady());
6117         assertFalse(calibrator.isRunning());
6118         assertNull(calibrator.getEstimatedBiases());
6119         assertFalse(calibrator.getEstimatedBiases(null));
6120         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6121         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6122         assertNull(calibrator.getEstimatedBiasFx());
6123         assertNull(calibrator.getEstimatedBiasFy());
6124         assertNull(calibrator.getEstimatedBiasFz());
6125         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6126         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6127         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6128         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6129         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6130         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6131         assertNull(calibrator.getEstimatedMa());
6132         assertNull(calibrator.getEstimatedSx());
6133         assertNull(calibrator.getEstimatedSy());
6134         assertNull(calibrator.getEstimatedSz());
6135         assertNull(calibrator.getEstimatedMxy());
6136         assertNull(calibrator.getEstimatedMxz());
6137         assertNull(calibrator.getEstimatedMyx());
6138         assertNull(calibrator.getEstimatedMyz());
6139         assertNull(calibrator.getEstimatedMzx());
6140         assertNull(calibrator.getEstimatedMzy());
6141         assertNull(calibrator.getEstimatedCovariance());
6142         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6143 
6144         // Force IllegalArgumentException
6145         calibrator = null;
6146         try {
6147             calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6148                     this);
6149             fail("IllegalArgumentException expected but not thrown");
6150         } catch (final IllegalArgumentException ignore) {
6151         }
6152         assertNull(calibrator);
6153     }
6154 
6155     @Test
6156     public void testConstructor57() throws WrongSizeException {
6157         final Collection<StandardDeviationBodyKinematics> measurements =
6158                 Collections.emptyList();
6159 
6160         final Matrix ba = generateBa();
6161         final double[] bias = ba.getBuffer();
6162         final double biasX = ba.getElementAtIndex(0);
6163         final double biasY = ba.getElementAtIndex(1);
6164         final double biasZ = ba.getElementAtIndex(2);
6165 
6166         KnownPositionAccelerometerCalibrator calibrator =
6167                 new KnownPositionAccelerometerCalibrator(measurements, bias);
6168 
6169         // check default values
6170         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6171         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6172         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6173         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6174         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6175         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6176         final Acceleration bx2 = new Acceleration(0.0,
6177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6178         calibrator.getInitialBiasXAsAcceleration(bx2);
6179         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6180         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6181         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6182         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6183         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6184         final Acceleration by2 = new Acceleration(0.0,
6185                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6186         calibrator.getInitialBiasYAsAcceleration(by2);
6187         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6188         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6189         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6190         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6191         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6192         final Acceleration bz2 = new Acceleration(0.0,
6193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6194         calibrator.getInitialBiasZAsAcceleration(bz2);
6195         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6196         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6197         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6198         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6199         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6200         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6201         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6202         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6203         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6204         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6205         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6206         final double[] bias1 = calibrator.getInitialBias();
6207         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6208         final double[] bias2 = new double[3];
6209         calibrator.getInitialBias(bias2);
6210         assertArrayEquals(bias1, bias2, 0.0);
6211         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6212         assertEquals(b1, ba);
6213         final Matrix b2 = new Matrix(3, 1);
6214         calibrator.getInitialBiasAsMatrix(b2);
6215         assertEquals(b1, b2);
6216         final Matrix ma1 = calibrator.getInitialMa();
6217         assertEquals(ma1, new Matrix(3, 3));
6218         final Matrix ma2 = new Matrix(3, 3);
6219         calibrator.getInitialMa(ma2);
6220         assertEquals(ma1, ma2);
6221         assertNull(calibrator.getEcefPosition());
6222         assertNull(calibrator.getNedPosition());
6223         assertFalse(calibrator.getNedPosition(null));
6224         assertSame(calibrator.getMeasurements(), measurements);
6225         assertFalse(calibrator.isCommonAxisUsed());
6226         assertNull(calibrator.getListener());
6227         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6228         assertFalse(calibrator.isReady());
6229         assertFalse(calibrator.isRunning());
6230         assertNull(calibrator.getEstimatedBiases());
6231         assertFalse(calibrator.getEstimatedBiases(null));
6232         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6233         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6234         assertNull(calibrator.getEstimatedBiasFx());
6235         assertNull(calibrator.getEstimatedBiasFy());
6236         assertNull(calibrator.getEstimatedBiasFz());
6237         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6238         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6239         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6240         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6241         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6242         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6243         assertNull(calibrator.getEstimatedMa());
6244         assertNull(calibrator.getEstimatedSx());
6245         assertNull(calibrator.getEstimatedSy());
6246         assertNull(calibrator.getEstimatedSz());
6247         assertNull(calibrator.getEstimatedMxy());
6248         assertNull(calibrator.getEstimatedMxz());
6249         assertNull(calibrator.getEstimatedMyx());
6250         assertNull(calibrator.getEstimatedMyz());
6251         assertNull(calibrator.getEstimatedMzx());
6252         assertNull(calibrator.getEstimatedMzy());
6253         assertNull(calibrator.getEstimatedCovariance());
6254         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6255 
6256         // Force IllegalArgumentException
6257         calibrator = null;
6258         try {
6259             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6260                     new double[1]);
6261             fail("IllegalArgumentException expected but not thrown");
6262         } catch (final IllegalArgumentException ignore) {
6263         }
6264         assertNull(calibrator);
6265     }
6266 
6267     @Test
6268     public void testConstructor58() throws WrongSizeException {
6269         final Collection<StandardDeviationBodyKinematics> measurements =
6270                 Collections.emptyList();
6271 
6272         final Matrix ba = generateBa();
6273         final double[] bias = ba.getBuffer();
6274         final double biasX = ba.getElementAtIndex(0);
6275         final double biasY = ba.getElementAtIndex(1);
6276         final double biasZ = ba.getElementAtIndex(2);
6277 
6278         KnownPositionAccelerometerCalibrator calibrator =
6279                 new KnownPositionAccelerometerCalibrator(measurements, bias,
6280                         this);
6281 
6282         // check default values
6283         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6284         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6285         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6286         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6287         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6288         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6289         final Acceleration bx2 = new Acceleration(0.0,
6290                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6291         calibrator.getInitialBiasXAsAcceleration(bx2);
6292         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6293         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6294         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6295         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6296         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6297         final Acceleration by2 = new Acceleration(0.0,
6298                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6299         calibrator.getInitialBiasYAsAcceleration(by2);
6300         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6301         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6302         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6303         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6304         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6305         final Acceleration bz2 = new Acceleration(0.0,
6306                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6307         calibrator.getInitialBiasZAsAcceleration(bz2);
6308         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6309         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6310         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6311         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6312         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6313         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6314         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6315         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6316         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6317         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6318         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6319         final double[] bias1 = calibrator.getInitialBias();
6320         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6321         final double[] bias2 = new double[3];
6322         calibrator.getInitialBias(bias2);
6323         assertArrayEquals(bias1, bias2, 0.0);
6324         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6325         assertEquals(b1, ba);
6326         final Matrix b2 = new Matrix(3, 1);
6327         calibrator.getInitialBiasAsMatrix(b2);
6328         assertEquals(b1, b2);
6329         final Matrix ma1 = calibrator.getInitialMa();
6330         assertEquals(ma1, new Matrix(3, 3));
6331         final Matrix ma2 = new Matrix(3, 3);
6332         calibrator.getInitialMa(ma2);
6333         assertEquals(ma1, ma2);
6334         assertNull(calibrator.getEcefPosition());
6335         assertNull(calibrator.getNedPosition());
6336         assertFalse(calibrator.getNedPosition(null));
6337         assertSame(calibrator.getMeasurements(), measurements);
6338         assertFalse(calibrator.isCommonAxisUsed());
6339         assertSame(calibrator.getListener(), this);
6340         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6341         assertFalse(calibrator.isReady());
6342         assertFalse(calibrator.isRunning());
6343         assertNull(calibrator.getEstimatedBiases());
6344         assertFalse(calibrator.getEstimatedBiases(null));
6345         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6346         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6347         assertNull(calibrator.getEstimatedBiasFx());
6348         assertNull(calibrator.getEstimatedBiasFy());
6349         assertNull(calibrator.getEstimatedBiasFz());
6350         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6351         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6352         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6353         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6354         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6355         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6356         assertNull(calibrator.getEstimatedMa());
6357         assertNull(calibrator.getEstimatedSx());
6358         assertNull(calibrator.getEstimatedSy());
6359         assertNull(calibrator.getEstimatedSz());
6360         assertNull(calibrator.getEstimatedMxy());
6361         assertNull(calibrator.getEstimatedMxz());
6362         assertNull(calibrator.getEstimatedMyx());
6363         assertNull(calibrator.getEstimatedMyz());
6364         assertNull(calibrator.getEstimatedMzx());
6365         assertNull(calibrator.getEstimatedMzy());
6366         assertNull(calibrator.getEstimatedCovariance());
6367         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6368 
6369         // Force IllegalArgumentException
6370         calibrator = null;
6371         try {
6372             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6373                     new double[1], this);
6374             fail("IllegalArgumentException expected but not thrown");
6375         } catch (final IllegalArgumentException ignore) {
6376         }
6377         assertNull(calibrator);
6378     }
6379 
6380     @Test
6381     public void testConstructor59() throws WrongSizeException {
6382         final Matrix ba = generateBa();
6383         final double[] bias = ba.getBuffer();
6384         final double biasX = ba.getElementAtIndex(0);
6385         final double biasY = ba.getElementAtIndex(1);
6386         final double biasZ = ba.getElementAtIndex(2);
6387 
6388         KnownPositionAccelerometerCalibrator calibrator =
6389                 new KnownPositionAccelerometerCalibrator(true, bias);
6390 
6391         // check default values
6392         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6393         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6394         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6395         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6396         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6397         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6398         final Acceleration bx2 = new Acceleration(0.0,
6399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6400         calibrator.getInitialBiasXAsAcceleration(bx2);
6401         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6402         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6403         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6404         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6405         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6406         final Acceleration by2 = new Acceleration(0.0,
6407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6408         calibrator.getInitialBiasYAsAcceleration(by2);
6409         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6410         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6411         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6412         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6413         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6414         final Acceleration bz2 = new Acceleration(0.0,
6415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6416         calibrator.getInitialBiasZAsAcceleration(bz2);
6417         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6418         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6419         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6420         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6421         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6422         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6423         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6424         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6425         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6426         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6427         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6428         final double[] bias1 = calibrator.getInitialBias();
6429         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6430         final double[] bias2 = new double[3];
6431         calibrator.getInitialBias(bias2);
6432         assertArrayEquals(bias1, bias2, 0.0);
6433         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6434         assertEquals(b1, ba);
6435         final Matrix b2 = new Matrix(3, 1);
6436         calibrator.getInitialBiasAsMatrix(b2);
6437         assertEquals(b1, b2);
6438         final Matrix ma1 = calibrator.getInitialMa();
6439         assertEquals(ma1, new Matrix(3, 3));
6440         final Matrix ma2 = new Matrix(3, 3);
6441         calibrator.getInitialMa(ma2);
6442         assertEquals(ma1, ma2);
6443         assertNull(calibrator.getEcefPosition());
6444         assertNull(calibrator.getNedPosition());
6445         assertFalse(calibrator.getNedPosition(null));
6446         assertNull(calibrator.getMeasurements());
6447         assertTrue(calibrator.isCommonAxisUsed());
6448         assertNull(calibrator.getListener());
6449         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6450         assertFalse(calibrator.isReady());
6451         assertFalse(calibrator.isRunning());
6452         assertNull(calibrator.getEstimatedBiases());
6453         assertFalse(calibrator.getEstimatedBiases(null));
6454         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6455         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6456         assertNull(calibrator.getEstimatedBiasFx());
6457         assertNull(calibrator.getEstimatedBiasFy());
6458         assertNull(calibrator.getEstimatedBiasFz());
6459         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6460         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6461         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6462         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6463         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6464         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6465         assertNull(calibrator.getEstimatedMa());
6466         assertNull(calibrator.getEstimatedSx());
6467         assertNull(calibrator.getEstimatedSy());
6468         assertNull(calibrator.getEstimatedSz());
6469         assertNull(calibrator.getEstimatedMxy());
6470         assertNull(calibrator.getEstimatedMxz());
6471         assertNull(calibrator.getEstimatedMyx());
6472         assertNull(calibrator.getEstimatedMyz());
6473         assertNull(calibrator.getEstimatedMzx());
6474         assertNull(calibrator.getEstimatedMzy());
6475         assertNull(calibrator.getEstimatedCovariance());
6476         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6477 
6478         // Force IllegalArgumentException
6479         calibrator = null;
6480         try {
6481             calibrator = new KnownPositionAccelerometerCalibrator(true,
6482                     new double[1]);
6483             fail("IllegalArgumentException expected but not thrown");
6484         } catch (final IllegalArgumentException ignore) {
6485         }
6486         assertNull(calibrator);
6487     }
6488 
6489     @Test
6490     public void testConstructor60() throws WrongSizeException {
6491         final Matrix ba = generateBa();
6492         final double[] bias = ba.getBuffer();
6493         final double biasX = ba.getElementAtIndex(0);
6494         final double biasY = ba.getElementAtIndex(1);
6495         final double biasZ = ba.getElementAtIndex(2);
6496 
6497         KnownPositionAccelerometerCalibrator calibrator =
6498                 new KnownPositionAccelerometerCalibrator(true, bias, this);
6499 
6500         // check default values
6501         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6502         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6503         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6504         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6505         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6506         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6507         final Acceleration bx2 = new Acceleration(0.0,
6508                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6509         calibrator.getInitialBiasXAsAcceleration(bx2);
6510         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6511         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6512         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6513         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6514         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6515         final Acceleration by2 = new Acceleration(0.0,
6516                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6517         calibrator.getInitialBiasYAsAcceleration(by2);
6518         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6519         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6520         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6521         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6522         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6523         final Acceleration bz2 = new Acceleration(0.0,
6524                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6525         calibrator.getInitialBiasZAsAcceleration(bz2);
6526         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6527         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6528         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6529         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6530         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6531         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6532         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6533         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6534         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6535         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6536         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6537         final double[] bias1 = calibrator.getInitialBias();
6538         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6539         final double[] bias2 = new double[3];
6540         calibrator.getInitialBias(bias2);
6541         assertArrayEquals(bias1, bias2, 0.0);
6542         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6543         assertEquals(b1, ba);
6544         final Matrix b2 = new Matrix(3, 1);
6545         calibrator.getInitialBiasAsMatrix(b2);
6546         assertEquals(b1, b2);
6547         final Matrix ma1 = calibrator.getInitialMa();
6548         assertEquals(ma1, new Matrix(3, 3));
6549         final Matrix ma2 = new Matrix(3, 3);
6550         calibrator.getInitialMa(ma2);
6551         assertEquals(ma1, ma2);
6552         assertNull(calibrator.getEcefPosition());
6553         assertNull(calibrator.getNedPosition());
6554         assertFalse(calibrator.getNedPosition(null));
6555         assertNull(calibrator.getMeasurements());
6556         assertTrue(calibrator.isCommonAxisUsed());
6557         assertSame(calibrator.getListener(), this);
6558         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6559         assertFalse(calibrator.isReady());
6560         assertFalse(calibrator.isRunning());
6561         assertNull(calibrator.getEstimatedBiases());
6562         assertFalse(calibrator.getEstimatedBiases(null));
6563         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6564         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6565         assertNull(calibrator.getEstimatedBiasFx());
6566         assertNull(calibrator.getEstimatedBiasFy());
6567         assertNull(calibrator.getEstimatedBiasFz());
6568         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6569         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6570         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6571         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6572         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6573         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6574         assertNull(calibrator.getEstimatedMa());
6575         assertNull(calibrator.getEstimatedSx());
6576         assertNull(calibrator.getEstimatedSy());
6577         assertNull(calibrator.getEstimatedSz());
6578         assertNull(calibrator.getEstimatedMxy());
6579         assertNull(calibrator.getEstimatedMxz());
6580         assertNull(calibrator.getEstimatedMyx());
6581         assertNull(calibrator.getEstimatedMyz());
6582         assertNull(calibrator.getEstimatedMzx());
6583         assertNull(calibrator.getEstimatedMzy());
6584         assertNull(calibrator.getEstimatedCovariance());
6585         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6586 
6587         // Force IllegalArgumentException
6588         calibrator = null;
6589         try {
6590             calibrator = new KnownPositionAccelerometerCalibrator(true,
6591                     new double[1], this);
6592             fail("IllegalArgumentException expected but not thrown");
6593         } catch (final IllegalArgumentException ignore) {
6594         }
6595         assertNull(calibrator);
6596     }
6597 
6598     @Test
6599     public void testConstructor61() throws WrongSizeException {
6600         final Collection<StandardDeviationBodyKinematics> measurements =
6601                 Collections.emptyList();
6602 
6603         final Matrix ba = generateBa();
6604         final double[] bias = ba.getBuffer();
6605         final double biasX = ba.getElementAtIndex(0);
6606         final double biasY = ba.getElementAtIndex(1);
6607         final double biasZ = ba.getElementAtIndex(2);
6608 
6609         KnownPositionAccelerometerCalibrator calibrator =
6610                 new KnownPositionAccelerometerCalibrator(measurements,
6611                         true, bias);
6612 
6613         // check default values
6614         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6615         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6616         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6617         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6618         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6619         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6620         final Acceleration bx2 = new Acceleration(0.0,
6621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6622         calibrator.getInitialBiasXAsAcceleration(bx2);
6623         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6624         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6625         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6626         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6627         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6628         final Acceleration by2 = new Acceleration(0.0,
6629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6630         calibrator.getInitialBiasYAsAcceleration(by2);
6631         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6632         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6633         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6634         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6635         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6636         final Acceleration bz2 = new Acceleration(0.0,
6637                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6638         calibrator.getInitialBiasZAsAcceleration(bz2);
6639         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6640         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6641         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6642         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6643         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6644         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6645         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6646         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6647         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6648         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6649         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6650         final double[] bias1 = calibrator.getInitialBias();
6651         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6652         final double[] bias2 = new double[3];
6653         calibrator.getInitialBias(bias2);
6654         assertArrayEquals(bias1, bias2, 0.0);
6655         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6656         assertEquals(b1, ba);
6657         final Matrix b2 = new Matrix(3, 1);
6658         calibrator.getInitialBiasAsMatrix(b2);
6659         assertEquals(b1, b2);
6660         final Matrix ma1 = calibrator.getInitialMa();
6661         assertEquals(ma1, new Matrix(3, 3));
6662         final Matrix ma2 = new Matrix(3, 3);
6663         calibrator.getInitialMa(ma2);
6664         assertEquals(ma1, ma2);
6665         assertNull(calibrator.getEcefPosition());
6666         assertNull(calibrator.getNedPosition());
6667         assertFalse(calibrator.getNedPosition(null));
6668         assertSame(calibrator.getMeasurements(), measurements);
6669         assertTrue(calibrator.isCommonAxisUsed());
6670         assertNull(calibrator.getListener());
6671         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6672         assertFalse(calibrator.isReady());
6673         assertFalse(calibrator.isRunning());
6674         assertNull(calibrator.getEstimatedBiases());
6675         assertFalse(calibrator.getEstimatedBiases(null));
6676         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6677         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6678         assertNull(calibrator.getEstimatedBiasFx());
6679         assertNull(calibrator.getEstimatedBiasFy());
6680         assertNull(calibrator.getEstimatedBiasFz());
6681         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6682         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6683         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6684         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6685         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6686         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6687         assertNull(calibrator.getEstimatedMa());
6688         assertNull(calibrator.getEstimatedSx());
6689         assertNull(calibrator.getEstimatedSy());
6690         assertNull(calibrator.getEstimatedSz());
6691         assertNull(calibrator.getEstimatedMxy());
6692         assertNull(calibrator.getEstimatedMxz());
6693         assertNull(calibrator.getEstimatedMyx());
6694         assertNull(calibrator.getEstimatedMyz());
6695         assertNull(calibrator.getEstimatedMzx());
6696         assertNull(calibrator.getEstimatedMzy());
6697         assertNull(calibrator.getEstimatedCovariance());
6698         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6699 
6700         // Force IllegalArgumentException
6701         calibrator = null;
6702         try {
6703             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6704                     true, new double[1]);
6705             fail("IllegalArgumentException expected but not thrown");
6706         } catch (final IllegalArgumentException ignore) {
6707         }
6708         assertNull(calibrator);
6709     }
6710 
6711     @Test
6712     public void testConstructor62() throws WrongSizeException {
6713         final Collection<StandardDeviationBodyKinematics> measurements =
6714                 Collections.emptyList();
6715 
6716         final Matrix ba = generateBa();
6717         final double[] bias = ba.getBuffer();
6718         final double biasX = ba.getElementAtIndex(0);
6719         final double biasY = ba.getElementAtIndex(1);
6720         final double biasZ = ba.getElementAtIndex(2);
6721 
6722         KnownPositionAccelerometerCalibrator calibrator =
6723                 new KnownPositionAccelerometerCalibrator(measurements,
6724                         true, bias, this);
6725 
6726         // check default values
6727         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6728         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6729         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6730         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6731         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6732         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6733         final Acceleration bx2 = new Acceleration(0.0,
6734                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6735         calibrator.getInitialBiasXAsAcceleration(bx2);
6736         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6737         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6738         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6739         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6740         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6741         final Acceleration by2 = new Acceleration(0.0,
6742                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6743         calibrator.getInitialBiasYAsAcceleration(by2);
6744         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6745         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6746         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6747         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6748         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6749         final Acceleration bz2 = new Acceleration(0.0,
6750                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6751         calibrator.getInitialBiasZAsAcceleration(bz2);
6752         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6753         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6754         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6755         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6756         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6757         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6758         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6759         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6760         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6761         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6762         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6763         final double[] bias1 = calibrator.getInitialBias();
6764         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6765         final double[] bias2 = new double[3];
6766         calibrator.getInitialBias(bias2);
6767         assertArrayEquals(bias1, bias2, 0.0);
6768         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6769         assertEquals(b1, ba);
6770         final Matrix b2 = new Matrix(3, 1);
6771         calibrator.getInitialBiasAsMatrix(b2);
6772         assertEquals(b1, b2);
6773         final Matrix ma1 = calibrator.getInitialMa();
6774         assertEquals(ma1, new Matrix(3, 3));
6775         final Matrix ma2 = new Matrix(3, 3);
6776         calibrator.getInitialMa(ma2);
6777         assertEquals(ma1, ma2);
6778         assertNull(calibrator.getEcefPosition());
6779         assertNull(calibrator.getNedPosition());
6780         assertFalse(calibrator.getNedPosition(null));
6781         assertSame(calibrator.getMeasurements(), measurements);
6782         assertTrue(calibrator.isCommonAxisUsed());
6783         assertSame(calibrator.getListener(), this);
6784         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6785         assertFalse(calibrator.isReady());
6786         assertFalse(calibrator.isRunning());
6787         assertNull(calibrator.getEstimatedBiases());
6788         assertFalse(calibrator.getEstimatedBiases(null));
6789         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6790         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6791         assertNull(calibrator.getEstimatedBiasFx());
6792         assertNull(calibrator.getEstimatedBiasFy());
6793         assertNull(calibrator.getEstimatedBiasFz());
6794         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6795         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6796         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6797         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6798         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6799         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6800         assertNull(calibrator.getEstimatedMa());
6801         assertNull(calibrator.getEstimatedSx());
6802         assertNull(calibrator.getEstimatedSy());
6803         assertNull(calibrator.getEstimatedSz());
6804         assertNull(calibrator.getEstimatedMxy());
6805         assertNull(calibrator.getEstimatedMxz());
6806         assertNull(calibrator.getEstimatedMyx());
6807         assertNull(calibrator.getEstimatedMyz());
6808         assertNull(calibrator.getEstimatedMzx());
6809         assertNull(calibrator.getEstimatedMzy());
6810         assertNull(calibrator.getEstimatedCovariance());
6811         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6812 
6813         // Force IllegalArgumentException
6814         calibrator = null;
6815         try {
6816             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6817                     true, new double[1], this);
6818             fail("IllegalArgumentException expected but not thrown");
6819         } catch (final IllegalArgumentException ignore) {
6820         }
6821         assertNull(calibrator);
6822     }
6823 
6824     @Test
6825     public void testConstructor63() throws WrongSizeException {
6826         final Matrix ba = generateBa();
6827         final double biasX = ba.getElementAtIndex(0);
6828         final double biasY = ba.getElementAtIndex(1);
6829         final double biasZ = ba.getElementAtIndex(2);
6830 
6831         KnownPositionAccelerometerCalibrator calibrator =
6832                 new KnownPositionAccelerometerCalibrator(ba);
6833 
6834         // check default values
6835         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6836         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6837         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6838         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6839         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6840         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6841         final Acceleration bx2 = new Acceleration(0.0,
6842                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6843         calibrator.getInitialBiasXAsAcceleration(bx2);
6844         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6845         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6846         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6847         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6848         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6849         final Acceleration by2 = new Acceleration(0.0,
6850                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6851         calibrator.getInitialBiasYAsAcceleration(by2);
6852         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6853         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6854         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6855         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6856         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6857         final Acceleration bz2 = new Acceleration(0.0,
6858                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6859         calibrator.getInitialBiasZAsAcceleration(bz2);
6860         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6861         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6862         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6863         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6864         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6865         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6866         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6867         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6868         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6869         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6870         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6871         final double[] bias1 = calibrator.getInitialBias();
6872         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6873         final double[] bias2 = new double[3];
6874         calibrator.getInitialBias(bias2);
6875         assertArrayEquals(bias1, bias2, 0.0);
6876         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6877         assertEquals(b1, ba);
6878         final Matrix b2 = new Matrix(3, 1);
6879         calibrator.getInitialBiasAsMatrix(b2);
6880         assertEquals(b1, b2);
6881         final Matrix ma1 = calibrator.getInitialMa();
6882         assertEquals(ma1, new Matrix(3, 3));
6883         final Matrix ma2 = new Matrix(3, 3);
6884         calibrator.getInitialMa(ma2);
6885         assertEquals(ma1, ma2);
6886         assertNull(calibrator.getEcefPosition());
6887         assertNull(calibrator.getNedPosition());
6888         assertFalse(calibrator.getNedPosition(null));
6889         assertNull(calibrator.getMeasurements());
6890         assertFalse(calibrator.isCommonAxisUsed());
6891         assertNull(calibrator.getListener());
6892         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6893         assertFalse(calibrator.isReady());
6894         assertFalse(calibrator.isRunning());
6895         assertNull(calibrator.getEstimatedBiases());
6896         assertFalse(calibrator.getEstimatedBiases(null));
6897         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6898         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6899         assertNull(calibrator.getEstimatedBiasFx());
6900         assertNull(calibrator.getEstimatedBiasFy());
6901         assertNull(calibrator.getEstimatedBiasFz());
6902         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6903         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6904         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6905         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6906         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6907         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6908         assertNull(calibrator.getEstimatedMa());
6909         assertNull(calibrator.getEstimatedSx());
6910         assertNull(calibrator.getEstimatedSy());
6911         assertNull(calibrator.getEstimatedSz());
6912         assertNull(calibrator.getEstimatedMxy());
6913         assertNull(calibrator.getEstimatedMxz());
6914         assertNull(calibrator.getEstimatedMyx());
6915         assertNull(calibrator.getEstimatedMyz());
6916         assertNull(calibrator.getEstimatedMzx());
6917         assertNull(calibrator.getEstimatedMzy());
6918         assertNull(calibrator.getEstimatedCovariance());
6919         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6920 
6921         // Force IllegalArgumentException
6922         calibrator = null;
6923         try {
6924             calibrator = new KnownPositionAccelerometerCalibrator(
6925                     new Matrix(1, 1));
6926             fail("IllegalArgumentException expected but not thrown");
6927         } catch (final IllegalArgumentException ignore) {
6928         }
6929         try {
6930             calibrator = new KnownPositionAccelerometerCalibrator(
6931                     new Matrix(1, 3));
6932             fail("IllegalArgumentException expected but not thrown");
6933         } catch (final IllegalArgumentException ignore) {
6934         }
6935         assertNull(calibrator);
6936     }
6937 
6938     @Test
6939     public void testConstructor64() throws WrongSizeException {
6940         final Matrix ba = generateBa();
6941         final double biasX = ba.getElementAtIndex(0);
6942         final double biasY = ba.getElementAtIndex(1);
6943         final double biasZ = ba.getElementAtIndex(2);
6944 
6945         KnownPositionAccelerometerCalibrator calibrator =
6946                 new KnownPositionAccelerometerCalibrator(ba, this);
6947 
6948         // check default values
6949         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6950         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6951         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6952         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6953         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6954         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6955         final Acceleration bx2 = new Acceleration(0.0,
6956                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6957         calibrator.getInitialBiasXAsAcceleration(bx2);
6958         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6959         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6960         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6961         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6962         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6963         final Acceleration by2 = new Acceleration(0.0,
6964                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6965         calibrator.getInitialBiasYAsAcceleration(by2);
6966         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6967         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6968         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6969         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6970         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6971         final Acceleration bz2 = new Acceleration(0.0,
6972                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6973         calibrator.getInitialBiasZAsAcceleration(bz2);
6974         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6975         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6976         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6977         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6978         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6979         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6980         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6981         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6982         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6983         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6984         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6985         final double[] bias1 = calibrator.getInitialBias();
6986         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
6987         final double[] bias2 = new double[3];
6988         calibrator.getInitialBias(bias2);
6989         assertArrayEquals(bias1, bias2, 0.0);
6990         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6991         assertEquals(b1, ba);
6992         final Matrix b2 = new Matrix(3, 1);
6993         calibrator.getInitialBiasAsMatrix(b2);
6994         assertEquals(b1, b2);
6995         final Matrix ma1 = calibrator.getInitialMa();
6996         assertEquals(ma1, new Matrix(3, 3));
6997         final Matrix ma2 = new Matrix(3, 3);
6998         calibrator.getInitialMa(ma2);
6999         assertEquals(ma1, ma2);
7000         assertNull(calibrator.getEcefPosition());
7001         assertNull(calibrator.getNedPosition());
7002         assertFalse(calibrator.getNedPosition(null));
7003         assertNull(calibrator.getMeasurements());
7004         assertFalse(calibrator.isCommonAxisUsed());
7005         assertSame(calibrator.getListener(), this);
7006         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7007         assertFalse(calibrator.isReady());
7008         assertFalse(calibrator.isRunning());
7009         assertNull(calibrator.getEstimatedBiases());
7010         assertFalse(calibrator.getEstimatedBiases(null));
7011         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7012         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7013         assertNull(calibrator.getEstimatedBiasFx());
7014         assertNull(calibrator.getEstimatedBiasFy());
7015         assertNull(calibrator.getEstimatedBiasFz());
7016         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7017         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7018         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7019         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7020         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7021         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7022         assertNull(calibrator.getEstimatedMa());
7023         assertNull(calibrator.getEstimatedSx());
7024         assertNull(calibrator.getEstimatedSy());
7025         assertNull(calibrator.getEstimatedSz());
7026         assertNull(calibrator.getEstimatedMxy());
7027         assertNull(calibrator.getEstimatedMxz());
7028         assertNull(calibrator.getEstimatedMyx());
7029         assertNull(calibrator.getEstimatedMyz());
7030         assertNull(calibrator.getEstimatedMzx());
7031         assertNull(calibrator.getEstimatedMzy());
7032         assertNull(calibrator.getEstimatedCovariance());
7033         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7034 
7035         // Force IllegalArgumentException
7036         calibrator = null;
7037         try {
7038             calibrator = new KnownPositionAccelerometerCalibrator(
7039                     new Matrix(1, 1));
7040             fail("IllegalArgumentException expected but not thrown");
7041         } catch (final IllegalArgumentException ignore) {
7042         }
7043         try {
7044             calibrator = new KnownPositionAccelerometerCalibrator(
7045                     new Matrix(1, 3));
7046             fail("IllegalArgumentException expected but not thrown");
7047         } catch (final IllegalArgumentException ignore) {
7048         }
7049         assertNull(calibrator);
7050     }
7051 
7052     @Test
7053     public void testConstructor65() throws WrongSizeException {
7054         final Collection<StandardDeviationBodyKinematics> measurements =
7055                 Collections.emptyList();
7056 
7057         final Matrix ba = generateBa();
7058         final double biasX = ba.getElementAtIndex(0);
7059         final double biasY = ba.getElementAtIndex(1);
7060         final double biasZ = ba.getElementAtIndex(2);
7061 
7062         KnownPositionAccelerometerCalibrator calibrator =
7063                 new KnownPositionAccelerometerCalibrator(measurements, ba);
7064 
7065         // check default values
7066         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7067         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7068         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7069         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7070         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7071         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7072         final Acceleration bx2 = new Acceleration(0.0,
7073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7074         calibrator.getInitialBiasXAsAcceleration(bx2);
7075         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7076         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7077         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7078         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7079         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7080         final Acceleration by2 = new Acceleration(0.0,
7081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7082         calibrator.getInitialBiasYAsAcceleration(by2);
7083         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7084         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7085         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7086         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7087         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7088         final Acceleration bz2 = new Acceleration(0.0,
7089                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7090         calibrator.getInitialBiasZAsAcceleration(bz2);
7091         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7092         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7093         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7094         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7095         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7096         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7097         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7098         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7099         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7100         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7101         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7102         final double[] bias1 = calibrator.getInitialBias();
7103         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7104         final double[] bias2 = new double[3];
7105         calibrator.getInitialBias(bias2);
7106         assertArrayEquals(bias1, bias2, 0.0);
7107         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7108         assertEquals(b1, ba);
7109         final Matrix b2 = new Matrix(3, 1);
7110         calibrator.getInitialBiasAsMatrix(b2);
7111         assertEquals(b1, b2);
7112         final Matrix ma1 = calibrator.getInitialMa();
7113         assertEquals(ma1, new Matrix(3, 3));
7114         final Matrix ma2 = new Matrix(3, 3);
7115         calibrator.getInitialMa(ma2);
7116         assertEquals(ma1, ma2);
7117         assertNull(calibrator.getEcefPosition());
7118         assertNull(calibrator.getNedPosition());
7119         assertFalse(calibrator.getNedPosition(null));
7120         assertSame(calibrator.getMeasurements(), measurements);
7121         assertFalse(calibrator.isCommonAxisUsed());
7122         assertNull(calibrator.getListener());
7123         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7124         assertFalse(calibrator.isReady());
7125         assertFalse(calibrator.isRunning());
7126         assertNull(calibrator.getEstimatedBiases());
7127         assertFalse(calibrator.getEstimatedBiases(null));
7128         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7129         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7130         assertNull(calibrator.getEstimatedBiasFx());
7131         assertNull(calibrator.getEstimatedBiasFy());
7132         assertNull(calibrator.getEstimatedBiasFz());
7133         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7134         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7135         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7136         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7137         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7138         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7139         assertNull(calibrator.getEstimatedMa());
7140         assertNull(calibrator.getEstimatedSx());
7141         assertNull(calibrator.getEstimatedSy());
7142         assertNull(calibrator.getEstimatedSz());
7143         assertNull(calibrator.getEstimatedMxy());
7144         assertNull(calibrator.getEstimatedMxz());
7145         assertNull(calibrator.getEstimatedMyx());
7146         assertNull(calibrator.getEstimatedMyz());
7147         assertNull(calibrator.getEstimatedMzx());
7148         assertNull(calibrator.getEstimatedMzy());
7149         assertNull(calibrator.getEstimatedCovariance());
7150         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7151 
7152         // Force IllegalArgumentException
7153         calibrator = null;
7154         try {
7155             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7156                     new Matrix(1, 1));
7157             fail("IllegalArgumentException expected but not thrown");
7158         } catch (final IllegalArgumentException ignore) {
7159         }
7160         try {
7161             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7162                     new Matrix(1, 3));
7163             fail("IllegalArgumentException expected but not thrown");
7164         } catch (final IllegalArgumentException ignore) {
7165         }
7166         assertNull(calibrator);
7167     }
7168 
7169     @Test
7170     public void testConstructor66() throws WrongSizeException {
7171         final Collection<StandardDeviationBodyKinematics> measurements =
7172                 Collections.emptyList();
7173 
7174         final Matrix ba = generateBa();
7175         final double biasX = ba.getElementAtIndex(0);
7176         final double biasY = ba.getElementAtIndex(1);
7177         final double biasZ = ba.getElementAtIndex(2);
7178 
7179         KnownPositionAccelerometerCalibrator calibrator =
7180                 new KnownPositionAccelerometerCalibrator(measurements, ba,
7181                         this);
7182 
7183         // check default values
7184         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7185         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7186         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7187         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7188         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7189         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7190         final Acceleration bx2 = new Acceleration(0.0,
7191                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7192         calibrator.getInitialBiasXAsAcceleration(bx2);
7193         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7194         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7195         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7196         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7197         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7198         final Acceleration by2 = new Acceleration(0.0,
7199                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7200         calibrator.getInitialBiasYAsAcceleration(by2);
7201         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7202         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7203         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7204         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7205         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7206         final Acceleration bz2 = new Acceleration(0.0,
7207                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7208         calibrator.getInitialBiasZAsAcceleration(bz2);
7209         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7210         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7211         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7212         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7213         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7214         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7215         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7216         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7217         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7218         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7219         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7220         final double[] bias1 = calibrator.getInitialBias();
7221         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7222         final double[] bias2 = new double[3];
7223         calibrator.getInitialBias(bias2);
7224         assertArrayEquals(bias1, bias2, 0.0);
7225         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7226         assertEquals(b1, ba);
7227         final Matrix b2 = new Matrix(3, 1);
7228         calibrator.getInitialBiasAsMatrix(b2);
7229         assertEquals(b1, b2);
7230         final Matrix ma1 = calibrator.getInitialMa();
7231         assertEquals(ma1, new Matrix(3, 3));
7232         final Matrix ma2 = new Matrix(3, 3);
7233         calibrator.getInitialMa(ma2);
7234         assertEquals(ma1, ma2);
7235         assertNull(calibrator.getEcefPosition());
7236         assertNull(calibrator.getNedPosition());
7237         assertFalse(calibrator.getNedPosition(null));
7238         assertSame(calibrator.getMeasurements(), measurements);
7239         assertFalse(calibrator.isCommonAxisUsed());
7240         assertSame(calibrator.getListener(), this);
7241         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7242         assertFalse(calibrator.isReady());
7243         assertFalse(calibrator.isRunning());
7244         assertNull(calibrator.getEstimatedBiases());
7245         assertFalse(calibrator.getEstimatedBiases(null));
7246         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7247         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7248         assertNull(calibrator.getEstimatedBiasFx());
7249         assertNull(calibrator.getEstimatedBiasFy());
7250         assertNull(calibrator.getEstimatedBiasFz());
7251         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7252         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7253         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7254         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7255         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7256         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7257         assertNull(calibrator.getEstimatedMa());
7258         assertNull(calibrator.getEstimatedSx());
7259         assertNull(calibrator.getEstimatedSy());
7260         assertNull(calibrator.getEstimatedSz());
7261         assertNull(calibrator.getEstimatedMxy());
7262         assertNull(calibrator.getEstimatedMxz());
7263         assertNull(calibrator.getEstimatedMyx());
7264         assertNull(calibrator.getEstimatedMyz());
7265         assertNull(calibrator.getEstimatedMzx());
7266         assertNull(calibrator.getEstimatedMzy());
7267         assertNull(calibrator.getEstimatedCovariance());
7268         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7269 
7270         // Force IllegalArgumentException
7271         calibrator = null;
7272         try {
7273             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7274                     new Matrix(1, 1), this);
7275             fail("IllegalArgumentException expected but not thrown");
7276         } catch (final IllegalArgumentException ignore) {
7277         }
7278         try {
7279             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7280                     new Matrix(1, 3), this);
7281             fail("IllegalArgumentException expected but not thrown");
7282         } catch (final IllegalArgumentException ignore) {
7283         }
7284         assertNull(calibrator);
7285     }
7286 
7287     @Test
7288     public void testConstructor67() throws WrongSizeException {
7289         final Matrix ba = generateBa();
7290         final double biasX = ba.getElementAtIndex(0);
7291         final double biasY = ba.getElementAtIndex(1);
7292         final double biasZ = ba.getElementAtIndex(2);
7293 
7294         KnownPositionAccelerometerCalibrator calibrator =
7295                 new KnownPositionAccelerometerCalibrator(true, ba);
7296 
7297         // check default values
7298         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7299         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7300         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7301         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7302         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7303         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7304         final Acceleration bx2 = new Acceleration(0.0,
7305                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7306         calibrator.getInitialBiasXAsAcceleration(bx2);
7307         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7308         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7309         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7310         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7311         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7312         final Acceleration by2 = new Acceleration(0.0,
7313                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7314         calibrator.getInitialBiasYAsAcceleration(by2);
7315         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7316         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7317         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7318         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7319         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7320         final Acceleration bz2 = new Acceleration(0.0,
7321                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7322         calibrator.getInitialBiasZAsAcceleration(bz2);
7323         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7324         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7325         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7326         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7327         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7328         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7329         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7330         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7331         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7332         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7333         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7334         final double[] bias1 = calibrator.getInitialBias();
7335         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7336         final double[] bias2 = new double[3];
7337         calibrator.getInitialBias(bias2);
7338         assertArrayEquals(bias1, bias2, 0.0);
7339         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7340         assertEquals(b1, ba);
7341         final Matrix b2 = new Matrix(3, 1);
7342         calibrator.getInitialBiasAsMatrix(b2);
7343         assertEquals(b1, b2);
7344         final Matrix ma1 = calibrator.getInitialMa();
7345         assertEquals(ma1, new Matrix(3, 3));
7346         final Matrix ma2 = new Matrix(3, 3);
7347         calibrator.getInitialMa(ma2);
7348         assertEquals(ma1, ma2);
7349         assertNull(calibrator.getEcefPosition());
7350         assertNull(calibrator.getNedPosition());
7351         assertFalse(calibrator.getNedPosition(null));
7352         assertNull(calibrator.getMeasurements());
7353         assertTrue(calibrator.isCommonAxisUsed());
7354         assertNull(calibrator.getListener());
7355         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7356         assertFalse(calibrator.isReady());
7357         assertFalse(calibrator.isRunning());
7358         assertNull(calibrator.getEstimatedBiases());
7359         assertFalse(calibrator.getEstimatedBiases(null));
7360         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7361         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7362         assertNull(calibrator.getEstimatedBiasFx());
7363         assertNull(calibrator.getEstimatedBiasFy());
7364         assertNull(calibrator.getEstimatedBiasFz());
7365         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7366         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7367         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7368         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7369         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7370         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7371         assertNull(calibrator.getEstimatedMa());
7372         assertNull(calibrator.getEstimatedSx());
7373         assertNull(calibrator.getEstimatedSy());
7374         assertNull(calibrator.getEstimatedSz());
7375         assertNull(calibrator.getEstimatedMxy());
7376         assertNull(calibrator.getEstimatedMxz());
7377         assertNull(calibrator.getEstimatedMyx());
7378         assertNull(calibrator.getEstimatedMyz());
7379         assertNull(calibrator.getEstimatedMzx());
7380         assertNull(calibrator.getEstimatedMzy());
7381         assertNull(calibrator.getEstimatedCovariance());
7382         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7383 
7384         // Force IllegalArgumentException
7385         calibrator = null;
7386         try {
7387             calibrator = new KnownPositionAccelerometerCalibrator(
7388                     true, new Matrix(1, 1));
7389             fail("IllegalArgumentException expected but not thrown");
7390         } catch (final IllegalArgumentException ignore) {
7391         }
7392         try {
7393             calibrator = new KnownPositionAccelerometerCalibrator(
7394                     true, new Matrix(1, 3));
7395             fail("IllegalArgumentException expected but not thrown");
7396         } catch (final IllegalArgumentException ignore) {
7397         }
7398         assertNull(calibrator);
7399     }
7400 
7401     @Test
7402     public void testConstructor68() throws WrongSizeException {
7403         final Matrix ba = generateBa();
7404         final double biasX = ba.getElementAtIndex(0);
7405         final double biasY = ba.getElementAtIndex(1);
7406         final double biasZ = ba.getElementAtIndex(2);
7407 
7408         KnownPositionAccelerometerCalibrator calibrator =
7409                 new KnownPositionAccelerometerCalibrator(true, ba,
7410                         this);
7411 
7412         // check default values
7413         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7414         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7415         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7416         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7417         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7418         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7419         final Acceleration bx2 = new Acceleration(0.0,
7420                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7421         calibrator.getInitialBiasXAsAcceleration(bx2);
7422         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7423         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7424         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7425         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7426         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7427         final Acceleration by2 = new Acceleration(0.0,
7428                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7429         calibrator.getInitialBiasYAsAcceleration(by2);
7430         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7431         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7432         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7433         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7434         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7435         final Acceleration bz2 = new Acceleration(0.0,
7436                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7437         calibrator.getInitialBiasZAsAcceleration(bz2);
7438         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7439         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7440         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7441         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7442         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7443         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7444         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7445         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7446         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7447         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7448         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7449         final double[] bias1 = calibrator.getInitialBias();
7450         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7451         final double[] bias2 = new double[3];
7452         calibrator.getInitialBias(bias2);
7453         assertArrayEquals(bias1, bias2, 0.0);
7454         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7455         assertEquals(b1, ba);
7456         final Matrix b2 = new Matrix(3, 1);
7457         calibrator.getInitialBiasAsMatrix(b2);
7458         assertEquals(b1, b2);
7459         final Matrix ma1 = calibrator.getInitialMa();
7460         assertEquals(ma1, new Matrix(3, 3));
7461         final Matrix ma2 = new Matrix(3, 3);
7462         calibrator.getInitialMa(ma2);
7463         assertEquals(ma1, ma2);
7464         assertNull(calibrator.getEcefPosition());
7465         assertNull(calibrator.getNedPosition());
7466         assertFalse(calibrator.getNedPosition(null));
7467         assertNull(calibrator.getMeasurements());
7468         assertTrue(calibrator.isCommonAxisUsed());
7469         assertSame(calibrator.getListener(), this);
7470         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7471         assertFalse(calibrator.isReady());
7472         assertFalse(calibrator.isRunning());
7473         assertNull(calibrator.getEstimatedBiases());
7474         assertFalse(calibrator.getEstimatedBiases(null));
7475         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7476         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7477         assertNull(calibrator.getEstimatedBiasFx());
7478         assertNull(calibrator.getEstimatedBiasFy());
7479         assertNull(calibrator.getEstimatedBiasFz());
7480         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7481         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7482         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7483         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7484         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7485         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7486         assertNull(calibrator.getEstimatedMa());
7487         assertNull(calibrator.getEstimatedSx());
7488         assertNull(calibrator.getEstimatedSy());
7489         assertNull(calibrator.getEstimatedSz());
7490         assertNull(calibrator.getEstimatedMxy());
7491         assertNull(calibrator.getEstimatedMxz());
7492         assertNull(calibrator.getEstimatedMyx());
7493         assertNull(calibrator.getEstimatedMyz());
7494         assertNull(calibrator.getEstimatedMzx());
7495         assertNull(calibrator.getEstimatedMzy());
7496         assertNull(calibrator.getEstimatedCovariance());
7497         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7498 
7499         // Force IllegalArgumentException
7500         calibrator = null;
7501         try {
7502             calibrator = new KnownPositionAccelerometerCalibrator(
7503                     true, new Matrix(1, 1),
7504                     this);
7505             fail("IllegalArgumentException expected but not thrown");
7506         } catch (final IllegalArgumentException ignore) {
7507         }
7508         try {
7509             calibrator = new KnownPositionAccelerometerCalibrator(
7510                     true, new Matrix(1, 3),
7511                     this);
7512             fail("IllegalArgumentException expected but not thrown");
7513         } catch (final IllegalArgumentException ignore) {
7514         }
7515         assertNull(calibrator);
7516     }
7517 
7518     @Test
7519     public void testConstructor69() throws WrongSizeException {
7520         final Collection<StandardDeviationBodyKinematics> measurements =
7521                 Collections.emptyList();
7522 
7523         final Matrix ba = generateBa();
7524         final double biasX = ba.getElementAtIndex(0);
7525         final double biasY = ba.getElementAtIndex(1);
7526         final double biasZ = ba.getElementAtIndex(2);
7527 
7528         KnownPositionAccelerometerCalibrator calibrator =
7529                 new KnownPositionAccelerometerCalibrator(measurements,
7530                         true, ba);
7531 
7532         // check default values
7533         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7534         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7535         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7536         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7537         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7538         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7539         final Acceleration bx2 = new Acceleration(0.0,
7540                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7541         calibrator.getInitialBiasXAsAcceleration(bx2);
7542         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7543         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7544         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7545         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7546         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7547         final Acceleration by2 = new Acceleration(0.0,
7548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7549         calibrator.getInitialBiasYAsAcceleration(by2);
7550         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7551         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7552         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7553         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7554         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7555         final Acceleration bz2 = new Acceleration(0.0,
7556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7557         calibrator.getInitialBiasZAsAcceleration(bz2);
7558         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7559         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7560         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7561         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7562         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7563         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7564         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7565         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7566         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7567         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7568         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7569         final double[] bias1 = calibrator.getInitialBias();
7570         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7571         final double[] bias2 = new double[3];
7572         calibrator.getInitialBias(bias2);
7573         assertArrayEquals(bias1, bias2, 0.0);
7574         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7575         assertEquals(b1, ba);
7576         final Matrix b2 = new Matrix(3, 1);
7577         calibrator.getInitialBiasAsMatrix(b2);
7578         assertEquals(b1, b2);
7579         final Matrix ma1 = calibrator.getInitialMa();
7580         assertEquals(ma1, new Matrix(3, 3));
7581         final Matrix ma2 = new Matrix(3, 3);
7582         calibrator.getInitialMa(ma2);
7583         assertEquals(ma1, ma2);
7584         assertNull(calibrator.getEcefPosition());
7585         assertNull(calibrator.getNedPosition());
7586         assertFalse(calibrator.getNedPosition(null));
7587         assertSame(calibrator.getMeasurements(), measurements);
7588         assertTrue(calibrator.isCommonAxisUsed());
7589         assertNull(calibrator.getListener());
7590         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7591         assertFalse(calibrator.isReady());
7592         assertFalse(calibrator.isRunning());
7593         assertNull(calibrator.getEstimatedBiases());
7594         assertFalse(calibrator.getEstimatedBiases(null));
7595         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7596         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7597         assertNull(calibrator.getEstimatedBiasFx());
7598         assertNull(calibrator.getEstimatedBiasFy());
7599         assertNull(calibrator.getEstimatedBiasFz());
7600         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7601         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7602         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7603         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7604         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7605         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7606         assertNull(calibrator.getEstimatedMa());
7607         assertNull(calibrator.getEstimatedSx());
7608         assertNull(calibrator.getEstimatedSy());
7609         assertNull(calibrator.getEstimatedSz());
7610         assertNull(calibrator.getEstimatedMxy());
7611         assertNull(calibrator.getEstimatedMxz());
7612         assertNull(calibrator.getEstimatedMyx());
7613         assertNull(calibrator.getEstimatedMyz());
7614         assertNull(calibrator.getEstimatedMzx());
7615         assertNull(calibrator.getEstimatedMzy());
7616         assertNull(calibrator.getEstimatedCovariance());
7617         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7618 
7619         // Force IllegalArgumentException
7620         calibrator = null;
7621         try {
7622             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7623                     true, new Matrix(1, 1));
7624             fail("IllegalArgumentException expected but not thrown");
7625         } catch (final IllegalArgumentException ignore) {
7626         }
7627         try {
7628             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7629                     true, new Matrix(1, 3));
7630             fail("IllegalArgumentException expected but not thrown");
7631         } catch (final IllegalArgumentException ignore) {
7632         }
7633         assertNull(calibrator);
7634     }
7635 
7636     @Test
7637     public void testConstructor70() throws WrongSizeException {
7638         final Collection<StandardDeviationBodyKinematics> measurements =
7639                 Collections.emptyList();
7640 
7641         final Matrix ba = generateBa();
7642         final double biasX = ba.getElementAtIndex(0);
7643         final double biasY = ba.getElementAtIndex(1);
7644         final double biasZ = ba.getElementAtIndex(2);
7645 
7646         KnownPositionAccelerometerCalibrator calibrator =
7647                 new KnownPositionAccelerometerCalibrator(measurements,
7648                         true, ba, this);
7649 
7650         // check default values
7651         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7652         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7653         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7654         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7655         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7656         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7657         final Acceleration bx2 = new Acceleration(0.0,
7658                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7659         calibrator.getInitialBiasXAsAcceleration(bx2);
7660         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7661         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7662         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7663         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7664         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7665         final Acceleration by2 = new Acceleration(0.0,
7666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7667         calibrator.getInitialBiasYAsAcceleration(by2);
7668         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7669         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7670         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7671         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7672         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7673         final Acceleration bz2 = new Acceleration(0.0,
7674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7675         calibrator.getInitialBiasZAsAcceleration(bz2);
7676         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7677         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7678         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7679         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7680         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7681         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7682         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7683         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7684         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7685         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7686         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7687         final double[] bias1 = calibrator.getInitialBias();
7688         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7689         final double[] bias2 = new double[3];
7690         calibrator.getInitialBias(bias2);
7691         assertArrayEquals(bias1, bias2, 0.0);
7692         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7693         assertEquals(b1, ba);
7694         final Matrix b2 = new Matrix(3, 1);
7695         calibrator.getInitialBiasAsMatrix(b2);
7696         assertEquals(b1, b2);
7697         final Matrix ma1 = calibrator.getInitialMa();
7698         assertEquals(ma1, new Matrix(3, 3));
7699         final Matrix ma2 = new Matrix(3, 3);
7700         calibrator.getInitialMa(ma2);
7701         assertEquals(ma1, ma2);
7702         assertNull(calibrator.getEcefPosition());
7703         assertNull(calibrator.getNedPosition());
7704         assertFalse(calibrator.getNedPosition(null));
7705         assertSame(calibrator.getMeasurements(), measurements);
7706         assertTrue(calibrator.isCommonAxisUsed());
7707         assertSame(calibrator.getListener(), this);
7708         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7709         assertFalse(calibrator.isReady());
7710         assertFalse(calibrator.isRunning());
7711         assertNull(calibrator.getEstimatedBiases());
7712         assertFalse(calibrator.getEstimatedBiases(null));
7713         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7714         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7715         assertNull(calibrator.getEstimatedBiasFx());
7716         assertNull(calibrator.getEstimatedBiasFy());
7717         assertNull(calibrator.getEstimatedBiasFz());
7718         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7719         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7720         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7721         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7722         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7723         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7724         assertNull(calibrator.getEstimatedMa());
7725         assertNull(calibrator.getEstimatedSx());
7726         assertNull(calibrator.getEstimatedSy());
7727         assertNull(calibrator.getEstimatedSz());
7728         assertNull(calibrator.getEstimatedMxy());
7729         assertNull(calibrator.getEstimatedMxz());
7730         assertNull(calibrator.getEstimatedMyx());
7731         assertNull(calibrator.getEstimatedMyz());
7732         assertNull(calibrator.getEstimatedMzx());
7733         assertNull(calibrator.getEstimatedMzy());
7734         assertNull(calibrator.getEstimatedCovariance());
7735         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7736 
7737         // Force IllegalArgumentException
7738         calibrator = null;
7739         try {
7740             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7741                     true, new Matrix(1, 1),
7742                     this);
7743             fail("IllegalArgumentException expected but not thrown");
7744         } catch (final IllegalArgumentException ignore) {
7745         }
7746         try {
7747             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7748                     true, new Matrix(1, 3),
7749                     this);
7750             fail("IllegalArgumentException expected but not thrown");
7751         } catch (final IllegalArgumentException ignore) {
7752         }
7753         assertNull(calibrator);
7754     }
7755 
7756     @Test
7757     public void testConstructor71() throws WrongSizeException {
7758         final Matrix ba = generateBa();
7759         final double biasX = ba.getElementAtIndex(0);
7760         final double biasY = ba.getElementAtIndex(1);
7761         final double biasZ = ba.getElementAtIndex(2);
7762 
7763         final Matrix ma = generateMaCommonAxis();
7764         final double sx = ma.getElementAt(0, 0);
7765         final double sy = ma.getElementAt(1, 1);
7766         final double sz = ma.getElementAt(2, 2);
7767         final double mxy = ma.getElementAt(0, 1);
7768         final double mxz = ma.getElementAt(0, 2);
7769         final double myx = ma.getElementAt(1, 0);
7770         final double myz = ma.getElementAt(1, 2);
7771         final double mzx = ma.getElementAt(2, 0);
7772         final double mzy = ma.getElementAt(2, 1);
7773 
7774         KnownPositionAccelerometerCalibrator calibrator =
7775                 new KnownPositionAccelerometerCalibrator(ba, ma);
7776 
7777         // check default values
7778         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7779         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7780         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7781         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7782         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7783         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7784         final Acceleration bx2 = new Acceleration(0.0,
7785                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7786         calibrator.getInitialBiasXAsAcceleration(bx2);
7787         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7788         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7789         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7790         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7791         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7792         final Acceleration by2 = new Acceleration(0.0,
7793                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7794         calibrator.getInitialBiasYAsAcceleration(by2);
7795         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7796         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7797         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7798         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7799         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7800         final Acceleration bz2 = new Acceleration(0.0,
7801                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7802         calibrator.getInitialBiasZAsAcceleration(bz2);
7803         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7804         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7805         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7806         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7807         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7808         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7809         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7810         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7811         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7812         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7813         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7814         final double[] bias1 = calibrator.getInitialBias();
7815         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7816         final double[] bias2 = new double[3];
7817         calibrator.getInitialBias(bias2);
7818         assertArrayEquals(bias1, bias2, 0.0);
7819         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7820         assertEquals(b1, ba);
7821         final Matrix b2 = new Matrix(3, 1);
7822         calibrator.getInitialBiasAsMatrix(b2);
7823         assertEquals(b1, b2);
7824         final Matrix ma1 = new Matrix(3, 3);
7825         ma1.setSubmatrix(0, 0,
7826                 2, 2,
7827                 new double[]{ sx, myx, mzx,
7828                         mxy, sy, mzy,
7829                         mxz, myz, sz});
7830         assertEquals(calibrator.getInitialMa(), ma1);
7831         final Matrix ma2 = new Matrix(3, 3);
7832         calibrator.getInitialMa(ma2);
7833         assertEquals(ma1, ma2);
7834         assertNull(calibrator.getEcefPosition());
7835         assertNull(calibrator.getNedPosition());
7836         assertFalse(calibrator.getNedPosition(null));
7837         assertNull(calibrator.getMeasurements());
7838         assertFalse(calibrator.isCommonAxisUsed());
7839         assertNull(calibrator.getListener());
7840         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7841         assertFalse(calibrator.isReady());
7842         assertFalse(calibrator.isRunning());
7843         assertNull(calibrator.getEstimatedBiases());
7844         assertFalse(calibrator.getEstimatedBiases(null));
7845         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7846         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7847         assertNull(calibrator.getEstimatedBiasFx());
7848         assertNull(calibrator.getEstimatedBiasFy());
7849         assertNull(calibrator.getEstimatedBiasFz());
7850         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7851         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7852         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7853         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7854         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7855         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7856         assertNull(calibrator.getEstimatedMa());
7857         assertNull(calibrator.getEstimatedSx());
7858         assertNull(calibrator.getEstimatedSy());
7859         assertNull(calibrator.getEstimatedSz());
7860         assertNull(calibrator.getEstimatedMxy());
7861         assertNull(calibrator.getEstimatedMxz());
7862         assertNull(calibrator.getEstimatedMyx());
7863         assertNull(calibrator.getEstimatedMyz());
7864         assertNull(calibrator.getEstimatedMzx());
7865         assertNull(calibrator.getEstimatedMzy());
7866         assertNull(calibrator.getEstimatedCovariance());
7867         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7868 
7869         // Force IllegalArgumentException
7870         calibrator = null;
7871         try {
7872             calibrator = new KnownPositionAccelerometerCalibrator(
7873                     new Matrix(1, 1), ma);
7874             fail("IllegalArgumentException expected but not thrown");
7875         } catch (final IllegalArgumentException ignore) {
7876         }
7877         try {
7878             calibrator = new KnownPositionAccelerometerCalibrator(
7879                     new Matrix(1, 3), ma);
7880             fail("IllegalArgumentException expected but not thrown");
7881         } catch (final IllegalArgumentException ignore) {
7882         }
7883         try {
7884             calibrator = new KnownPositionAccelerometerCalibrator(ba,
7885                     new Matrix(1, 3));
7886             fail("IllegalArgumentException expected but not thrown");
7887         } catch (final IllegalArgumentException ignore) {
7888         }
7889         try {
7890             calibrator = new KnownPositionAccelerometerCalibrator(ba,
7891                     new Matrix(3, 1));
7892             fail("IllegalArgumentException expected but not thrown");
7893         } catch (final IllegalArgumentException ignore) {
7894         }
7895         assertNull(calibrator);
7896     }
7897 
7898     @Test
7899     public void testConstructor72() throws WrongSizeException {
7900         final Matrix ba = generateBa();
7901         final double biasX = ba.getElementAtIndex(0);
7902         final double biasY = ba.getElementAtIndex(1);
7903         final double biasZ = ba.getElementAtIndex(2);
7904 
7905         final Matrix ma = generateMaCommonAxis();
7906         final double sx = ma.getElementAt(0, 0);
7907         final double sy = ma.getElementAt(1, 1);
7908         final double sz = ma.getElementAt(2, 2);
7909         final double mxy = ma.getElementAt(0, 1);
7910         final double mxz = ma.getElementAt(0, 2);
7911         final double myx = ma.getElementAt(1, 0);
7912         final double myz = ma.getElementAt(1, 2);
7913         final double mzx = ma.getElementAt(2, 0);
7914         final double mzy = ma.getElementAt(2, 1);
7915 
7916         KnownPositionAccelerometerCalibrator calibrator =
7917                 new KnownPositionAccelerometerCalibrator(ba, ma, this);
7918 
7919         // check default values
7920         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7921         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7922         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7923         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7924         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7926         final Acceleration bx2 = new Acceleration(0.0,
7927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7928         calibrator.getInitialBiasXAsAcceleration(bx2);
7929         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7930         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7931         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7932         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7933         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7934         final Acceleration by2 = new Acceleration(0.0,
7935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7936         calibrator.getInitialBiasYAsAcceleration(by2);
7937         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7938         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7939         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7940         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7941         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7942         final Acceleration bz2 = new Acceleration(0.0,
7943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7944         calibrator.getInitialBiasZAsAcceleration(bz2);
7945         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7946         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7947         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7948         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7949         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7950         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7951         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7952         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7953         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7954         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7955         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7956         final double[] bias1 = calibrator.getInitialBias();
7957         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
7958         final double[] bias2 = new double[3];
7959         calibrator.getInitialBias(bias2);
7960         assertArrayEquals(bias1, bias2, 0.0);
7961         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7962         assertEquals(b1, ba);
7963         final Matrix b2 = new Matrix(3, 1);
7964         calibrator.getInitialBiasAsMatrix(b2);
7965         assertEquals(b1, b2);
7966         final Matrix ma1 = new Matrix(3, 3);
7967         ma1.setSubmatrix(0, 0,
7968                 2, 2,
7969                 new double[]{ sx, myx, mzx,
7970                         mxy, sy, mzy,
7971                         mxz, myz, sz});
7972         assertEquals(calibrator.getInitialMa(), ma1);
7973         final Matrix ma2 = new Matrix(3, 3);
7974         calibrator.getInitialMa(ma2);
7975         assertEquals(ma1, ma2);
7976         assertNull(calibrator.getEcefPosition());
7977         assertNull(calibrator.getNedPosition());
7978         assertFalse(calibrator.getNedPosition(null));
7979         assertNull(calibrator.getMeasurements());
7980         assertFalse(calibrator.isCommonAxisUsed());
7981         assertSame(calibrator.getListener(), this);
7982         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7983         assertFalse(calibrator.isReady());
7984         assertFalse(calibrator.isRunning());
7985         assertNull(calibrator.getEstimatedBiases());
7986         assertFalse(calibrator.getEstimatedBiases(null));
7987         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7988         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7989         assertNull(calibrator.getEstimatedBiasFx());
7990         assertNull(calibrator.getEstimatedBiasFy());
7991         assertNull(calibrator.getEstimatedBiasFz());
7992         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7993         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7994         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7995         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7996         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7997         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7998         assertNull(calibrator.getEstimatedMa());
7999         assertNull(calibrator.getEstimatedSx());
8000         assertNull(calibrator.getEstimatedSy());
8001         assertNull(calibrator.getEstimatedSz());
8002         assertNull(calibrator.getEstimatedMxy());
8003         assertNull(calibrator.getEstimatedMxz());
8004         assertNull(calibrator.getEstimatedMyx());
8005         assertNull(calibrator.getEstimatedMyz());
8006         assertNull(calibrator.getEstimatedMzx());
8007         assertNull(calibrator.getEstimatedMzy());
8008         assertNull(calibrator.getEstimatedCovariance());
8009         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8010 
8011         // Force IllegalArgumentException
8012         calibrator = null;
8013         try {
8014             calibrator = new KnownPositionAccelerometerCalibrator(
8015                     new Matrix(1, 1), ma, this);
8016             fail("IllegalArgumentException expected but not thrown");
8017         } catch (final IllegalArgumentException ignore) {
8018         }
8019         try {
8020             calibrator = new KnownPositionAccelerometerCalibrator(
8021                     new Matrix(1, 3), ma, this);
8022             fail("IllegalArgumentException expected but not thrown");
8023         } catch (final IllegalArgumentException ignore) {
8024         }
8025         try {
8026             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8027                     new Matrix(1, 3), this);
8028             fail("IllegalArgumentException expected but not thrown");
8029         } catch (final IllegalArgumentException ignore) {
8030         }
8031         try {
8032             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8033                     new Matrix(3, 1), this);
8034             fail("IllegalArgumentException expected but not thrown");
8035         } catch (final IllegalArgumentException ignore) {
8036         }
8037         assertNull(calibrator);
8038     }
8039 
8040     @Test
8041     public void testConstructor73() throws WrongSizeException {
8042         final Collection<StandardDeviationBodyKinematics> measurements =
8043                 Collections.emptyList();
8044 
8045         final Matrix ba = generateBa();
8046         final double biasX = ba.getElementAtIndex(0);
8047         final double biasY = ba.getElementAtIndex(1);
8048         final double biasZ = ba.getElementAtIndex(2);
8049 
8050         final Matrix ma = generateMaCommonAxis();
8051         final double sx = ma.getElementAt(0, 0);
8052         final double sy = ma.getElementAt(1, 1);
8053         final double sz = ma.getElementAt(2, 2);
8054         final double mxy = ma.getElementAt(0, 1);
8055         final double mxz = ma.getElementAt(0, 2);
8056         final double myx = ma.getElementAt(1, 0);
8057         final double myz = ma.getElementAt(1, 2);
8058         final double mzx = ma.getElementAt(2, 0);
8059         final double mzy = ma.getElementAt(2, 1);
8060 
8061         KnownPositionAccelerometerCalibrator calibrator =
8062                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8063 
8064         // check default values
8065         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8066         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8067         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8068         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8069         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8070         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8071         final Acceleration bx2 = new Acceleration(0.0,
8072                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8073         calibrator.getInitialBiasXAsAcceleration(bx2);
8074         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8075         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8076         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8077         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8078         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8079         final Acceleration by2 = new Acceleration(0.0,
8080                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8081         calibrator.getInitialBiasYAsAcceleration(by2);
8082         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8083         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8084         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8085         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8086         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8087         final Acceleration bz2 = new Acceleration(0.0,
8088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8089         calibrator.getInitialBiasZAsAcceleration(bz2);
8090         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8091         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8092         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8093         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8094         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8095         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8096         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8097         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8098         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8099         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8100         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8101         final double[] bias1 = calibrator.getInitialBias();
8102         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8103         final double[] bias2 = new double[3];
8104         calibrator.getInitialBias(bias2);
8105         assertArrayEquals(bias1, bias2, 0.0);
8106         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8107         assertEquals(b1, ba);
8108         final Matrix b2 = new Matrix(3, 1);
8109         calibrator.getInitialBiasAsMatrix(b2);
8110         assertEquals(b1, b2);
8111         final Matrix ma1 = new Matrix(3, 3);
8112         ma1.setSubmatrix(0, 0,
8113                 2, 2,
8114                 new double[]{ sx, myx, mzx,
8115                         mxy, sy, mzy,
8116                         mxz, myz, sz});
8117         assertEquals(calibrator.getInitialMa(), ma1);
8118         final Matrix ma2 = new Matrix(3, 3);
8119         calibrator.getInitialMa(ma2);
8120         assertEquals(ma1, ma2);
8121         assertNull(calibrator.getEcefPosition());
8122         assertNull(calibrator.getNedPosition());
8123         assertFalse(calibrator.getNedPosition(null));
8124         assertSame(calibrator.getMeasurements(), measurements);
8125         assertFalse(calibrator.isCommonAxisUsed());
8126         assertNull(calibrator.getListener());
8127         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8128         assertFalse(calibrator.isReady());
8129         assertFalse(calibrator.isRunning());
8130         assertNull(calibrator.getEstimatedBiases());
8131         assertFalse(calibrator.getEstimatedBiases(null));
8132         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8133         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8134         assertNull(calibrator.getEstimatedBiasFx());
8135         assertNull(calibrator.getEstimatedBiasFy());
8136         assertNull(calibrator.getEstimatedBiasFz());
8137         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8138         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8139         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8140         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8141         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8142         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8143         assertNull(calibrator.getEstimatedMa());
8144         assertNull(calibrator.getEstimatedSx());
8145         assertNull(calibrator.getEstimatedSy());
8146         assertNull(calibrator.getEstimatedSz());
8147         assertNull(calibrator.getEstimatedMxy());
8148         assertNull(calibrator.getEstimatedMxz());
8149         assertNull(calibrator.getEstimatedMyx());
8150         assertNull(calibrator.getEstimatedMyz());
8151         assertNull(calibrator.getEstimatedMzx());
8152         assertNull(calibrator.getEstimatedMzy());
8153         assertNull(calibrator.getEstimatedCovariance());
8154         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8155 
8156         // Force IllegalArgumentException
8157         calibrator = null;
8158         try {
8159             calibrator = new KnownPositionAccelerometerCalibrator(
8160                     measurements, new Matrix(1, 1), ma);
8161             fail("IllegalArgumentException expected but not thrown");
8162         } catch (final IllegalArgumentException ignore) {
8163         }
8164         try {
8165             calibrator = new KnownPositionAccelerometerCalibrator(
8166                     measurements, new Matrix(1, 3), ma);
8167             fail("IllegalArgumentException expected but not thrown");
8168         } catch (final IllegalArgumentException ignore) {
8169         }
8170         try {
8171             calibrator = new KnownPositionAccelerometerCalibrator(
8172                     measurements, ba, new Matrix(1, 3));
8173             fail("IllegalArgumentException expected but not thrown");
8174         } catch (final IllegalArgumentException ignore) {
8175         }
8176         try {
8177             calibrator = new KnownPositionAccelerometerCalibrator(
8178                     measurements, ba, new Matrix(3, 1));
8179             fail("IllegalArgumentException expected but not thrown");
8180         } catch (final IllegalArgumentException ignore) {
8181         }
8182         assertNull(calibrator);
8183     }
8184 
8185     @Test
8186     public void testConstructor74() throws WrongSizeException {
8187         final Collection<StandardDeviationBodyKinematics> measurements =
8188                 Collections.emptyList();
8189 
8190         final Matrix ba = generateBa();
8191         final double biasX = ba.getElementAtIndex(0);
8192         final double biasY = ba.getElementAtIndex(1);
8193         final double biasZ = ba.getElementAtIndex(2);
8194 
8195         final Matrix ma = generateMaCommonAxis();
8196         final double sx = ma.getElementAt(0, 0);
8197         final double sy = ma.getElementAt(1, 1);
8198         final double sz = ma.getElementAt(2, 2);
8199         final double mxy = ma.getElementAt(0, 1);
8200         final double mxz = ma.getElementAt(0, 2);
8201         final double myx = ma.getElementAt(1, 0);
8202         final double myz = ma.getElementAt(1, 2);
8203         final double mzx = ma.getElementAt(2, 0);
8204         final double mzy = ma.getElementAt(2, 1);
8205 
8206         KnownPositionAccelerometerCalibrator calibrator =
8207                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8208                         this);
8209 
8210         // check default values
8211         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8212         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8213         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8214         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8215         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8216         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8217         final Acceleration bx2 = new Acceleration(0.0,
8218                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8219         calibrator.getInitialBiasXAsAcceleration(bx2);
8220         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8221         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8222         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8223         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8224         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8225         final Acceleration by2 = new Acceleration(0.0,
8226                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8227         calibrator.getInitialBiasYAsAcceleration(by2);
8228         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8229         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8230         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8231         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8232         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8233         final Acceleration bz2 = new Acceleration(0.0,
8234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8235         calibrator.getInitialBiasZAsAcceleration(bz2);
8236         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8237         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8238         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8239         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8240         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8241         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8242         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8243         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8244         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8245         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8246         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8247         final double[] bias1 = calibrator.getInitialBias();
8248         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8249         final double[] bias2 = new double[3];
8250         calibrator.getInitialBias(bias2);
8251         assertArrayEquals(bias1, bias2, 0.0);
8252         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8253         assertEquals(b1, ba);
8254         final Matrix b2 = new Matrix(3, 1);
8255         calibrator.getInitialBiasAsMatrix(b2);
8256         assertEquals(b1, b2);
8257         final Matrix ma1 = new Matrix(3, 3);
8258         ma1.setSubmatrix(0, 0,
8259                 2, 2,
8260                 new double[]{ sx, myx, mzx,
8261                         mxy, sy, mzy,
8262                         mxz, myz, sz});
8263         assertEquals(calibrator.getInitialMa(), ma1);
8264         final Matrix ma2 = new Matrix(3, 3);
8265         calibrator.getInitialMa(ma2);
8266         assertEquals(ma1, ma2);
8267         assertNull(calibrator.getEcefPosition());
8268         assertNull(calibrator.getNedPosition());
8269         assertFalse(calibrator.getNedPosition(null));
8270         assertSame(calibrator.getMeasurements(), measurements);
8271         assertFalse(calibrator.isCommonAxisUsed());
8272         assertSame(calibrator.getListener(), this);
8273         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8274         assertFalse(calibrator.isReady());
8275         assertFalse(calibrator.isRunning());
8276         assertNull(calibrator.getEstimatedBiases());
8277         assertFalse(calibrator.getEstimatedBiases(null));
8278         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8279         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8280         assertNull(calibrator.getEstimatedBiasFx());
8281         assertNull(calibrator.getEstimatedBiasFy());
8282         assertNull(calibrator.getEstimatedBiasFz());
8283         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8284         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8285         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8286         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8287         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8288         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8289         assertNull(calibrator.getEstimatedMa());
8290         assertNull(calibrator.getEstimatedSx());
8291         assertNull(calibrator.getEstimatedSy());
8292         assertNull(calibrator.getEstimatedSz());
8293         assertNull(calibrator.getEstimatedMxy());
8294         assertNull(calibrator.getEstimatedMxz());
8295         assertNull(calibrator.getEstimatedMyx());
8296         assertNull(calibrator.getEstimatedMyz());
8297         assertNull(calibrator.getEstimatedMzx());
8298         assertNull(calibrator.getEstimatedMzy());
8299         assertNull(calibrator.getEstimatedCovariance());
8300         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8301 
8302         // Force IllegalArgumentException
8303         calibrator = null;
8304         try {
8305             calibrator = new KnownPositionAccelerometerCalibrator(
8306                     measurements, new Matrix(1, 1), ma, this);
8307             fail("IllegalArgumentException expected but not thrown");
8308         } catch (final IllegalArgumentException ignore) {
8309         }
8310         try {
8311             calibrator = new KnownPositionAccelerometerCalibrator(
8312                     measurements, new Matrix(1, 3), ma, this);
8313             fail("IllegalArgumentException expected but not thrown");
8314         } catch (final IllegalArgumentException ignore) {
8315         }
8316         try {
8317             calibrator = new KnownPositionAccelerometerCalibrator(
8318                     measurements, ba, new Matrix(1, 3), this);
8319             fail("IllegalArgumentException expected but not thrown");
8320         } catch (final IllegalArgumentException ignore) {
8321         }
8322         try {
8323             calibrator = new KnownPositionAccelerometerCalibrator(
8324                     measurements, ba, new Matrix(3, 1), this);
8325             fail("IllegalArgumentException expected but not thrown");
8326         } catch (final IllegalArgumentException ignore) {
8327         }
8328         assertNull(calibrator);
8329     }
8330 
8331     @Test
8332     public void testConstructor75() throws WrongSizeException {
8333         final Matrix ba = generateBa();
8334         final double biasX = ba.getElementAtIndex(0);
8335         final double biasY = ba.getElementAtIndex(1);
8336         final double biasZ = ba.getElementAtIndex(2);
8337 
8338         final Matrix ma = generateMaCommonAxis();
8339         final double sx = ma.getElementAt(0, 0);
8340         final double sy = ma.getElementAt(1, 1);
8341         final double sz = ma.getElementAt(2, 2);
8342         final double mxy = ma.getElementAt(0, 1);
8343         final double mxz = ma.getElementAt(0, 2);
8344         final double myx = ma.getElementAt(1, 0);
8345         final double myz = ma.getElementAt(1, 2);
8346         final double mzx = ma.getElementAt(2, 0);
8347         final double mzy = ma.getElementAt(2, 1);
8348 
8349         KnownPositionAccelerometerCalibrator calibrator =
8350                 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8351 
8352         // check default values
8353         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8354         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8355         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8356         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8357         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8358         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8359         final Acceleration bx2 = new Acceleration(0.0,
8360                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8361         calibrator.getInitialBiasXAsAcceleration(bx2);
8362         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8363         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8364         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8365         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8366         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8367         final Acceleration by2 = new Acceleration(0.0,
8368                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8369         calibrator.getInitialBiasYAsAcceleration(by2);
8370         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8371         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8372         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8373         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8374         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8375         final Acceleration bz2 = new Acceleration(0.0,
8376                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8377         calibrator.getInitialBiasZAsAcceleration(bz2);
8378         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8379         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8380         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8381         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8382         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8383         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8384         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8385         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8386         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8387         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8388         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8389         final double[] bias1 = calibrator.getInitialBias();
8390         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8391         final double[] bias2 = new double[3];
8392         calibrator.getInitialBias(bias2);
8393         assertArrayEquals(bias1, bias2, 0.0);
8394         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8395         assertEquals(b1, ba);
8396         final Matrix b2 = new Matrix(3, 1);
8397         calibrator.getInitialBiasAsMatrix(b2);
8398         assertEquals(b1, b2);
8399         final Matrix ma1 = new Matrix(3, 3);
8400         ma1.setSubmatrix(0, 0,
8401                 2, 2,
8402                 new double[]{ sx, myx, mzx,
8403                         mxy, sy, mzy,
8404                         mxz, myz, sz});
8405         assertEquals(calibrator.getInitialMa(), ma1);
8406         final Matrix ma2 = new Matrix(3, 3);
8407         calibrator.getInitialMa(ma2);
8408         assertEquals(ma1, ma2);
8409         assertNull(calibrator.getEcefPosition());
8410         assertNull(calibrator.getNedPosition());
8411         assertFalse(calibrator.getNedPosition(null));
8412         assertNull(calibrator.getMeasurements());
8413         assertTrue(calibrator.isCommonAxisUsed());
8414         assertNull(calibrator.getListener());
8415         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8416         assertFalse(calibrator.isReady());
8417         assertFalse(calibrator.isRunning());
8418         assertNull(calibrator.getEstimatedBiases());
8419         assertFalse(calibrator.getEstimatedBiases(null));
8420         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8421         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8422         assertNull(calibrator.getEstimatedBiasFx());
8423         assertNull(calibrator.getEstimatedBiasFy());
8424         assertNull(calibrator.getEstimatedBiasFz());
8425         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8426         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8427         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8428         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8429         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8430         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8431         assertNull(calibrator.getEstimatedMa());
8432         assertNull(calibrator.getEstimatedSx());
8433         assertNull(calibrator.getEstimatedSy());
8434         assertNull(calibrator.getEstimatedSz());
8435         assertNull(calibrator.getEstimatedMxy());
8436         assertNull(calibrator.getEstimatedMxz());
8437         assertNull(calibrator.getEstimatedMyx());
8438         assertNull(calibrator.getEstimatedMyz());
8439         assertNull(calibrator.getEstimatedMzx());
8440         assertNull(calibrator.getEstimatedMzy());
8441         assertNull(calibrator.getEstimatedCovariance());
8442         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8443 
8444         // Force IllegalArgumentException
8445         calibrator = null;
8446         try {
8447             calibrator = new KnownPositionAccelerometerCalibrator(
8448                     true, new Matrix(1, 1), ma);
8449             fail("IllegalArgumentException expected but not thrown");
8450         } catch (final IllegalArgumentException ignore) {
8451         }
8452         try {
8453             calibrator = new KnownPositionAccelerometerCalibrator(
8454                     true, new Matrix(1, 3), ma);
8455             fail("IllegalArgumentException expected but not thrown");
8456         } catch (final IllegalArgumentException ignore) {
8457         }
8458         try {
8459             calibrator = new KnownPositionAccelerometerCalibrator(
8460                     true, ba, new Matrix(1, 3));
8461             fail("IllegalArgumentException expected but not thrown");
8462         } catch (final IllegalArgumentException ignore) {
8463         }
8464         try {
8465             calibrator = new KnownPositionAccelerometerCalibrator(
8466                     true, ba, new Matrix(3, 1));
8467             fail("IllegalArgumentException expected but not thrown");
8468         } catch (final IllegalArgumentException ignore) {
8469         }
8470         assertNull(calibrator);
8471     }
8472 
8473     @Test
8474     public void testConstructor76() throws WrongSizeException {
8475         final Matrix ba = generateBa();
8476         final double biasX = ba.getElementAtIndex(0);
8477         final double biasY = ba.getElementAtIndex(1);
8478         final double biasZ = ba.getElementAtIndex(2);
8479 
8480         final Matrix ma = generateMaCommonAxis();
8481         final double sx = ma.getElementAt(0, 0);
8482         final double sy = ma.getElementAt(1, 1);
8483         final double sz = ma.getElementAt(2, 2);
8484         final double mxy = ma.getElementAt(0, 1);
8485         final double mxz = ma.getElementAt(0, 2);
8486         final double myx = ma.getElementAt(1, 0);
8487         final double myz = ma.getElementAt(1, 2);
8488         final double mzx = ma.getElementAt(2, 0);
8489         final double mzy = ma.getElementAt(2, 1);
8490 
8491         KnownPositionAccelerometerCalibrator calibrator =
8492                 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8493                         this);
8494 
8495         // check default values
8496         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8497         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8498         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8499         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8500         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8501         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8502         final Acceleration bx2 = new Acceleration(0.0,
8503                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8504         calibrator.getInitialBiasXAsAcceleration(bx2);
8505         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8506         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8507         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8508         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8509         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8510         final Acceleration by2 = new Acceleration(0.0,
8511                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8512         calibrator.getInitialBiasYAsAcceleration(by2);
8513         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8514         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8515         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8516         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8517         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8518         final Acceleration bz2 = new Acceleration(0.0,
8519                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8520         calibrator.getInitialBiasZAsAcceleration(bz2);
8521         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8522         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8523         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8524         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8525         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8526         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8527         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8528         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8529         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8530         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8531         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8532         final double[] bias1 = calibrator.getInitialBias();
8533         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8534         final double[] bias2 = new double[3];
8535         calibrator.getInitialBias(bias2);
8536         assertArrayEquals(bias1, bias2, 0.0);
8537         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8538         assertEquals(b1, ba);
8539         final Matrix b2 = new Matrix(3, 1);
8540         calibrator.getInitialBiasAsMatrix(b2);
8541         assertEquals(b1, b2);
8542         final Matrix ma1 = new Matrix(3, 3);
8543         ma1.setSubmatrix(0, 0,
8544                 2, 2,
8545                 new double[]{ sx, myx, mzx,
8546                         mxy, sy, mzy,
8547                         mxz, myz, sz});
8548         assertEquals(calibrator.getInitialMa(), ma1);
8549         final Matrix ma2 = new Matrix(3, 3);
8550         calibrator.getInitialMa(ma2);
8551         assertEquals(ma1, ma2);
8552         assertNull(calibrator.getEcefPosition());
8553         assertNull(calibrator.getNedPosition());
8554         assertFalse(calibrator.getNedPosition(null));
8555         assertNull(calibrator.getMeasurements());
8556         assertTrue(calibrator.isCommonAxisUsed());
8557         assertSame(calibrator.getListener(), this);
8558         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8559         assertFalse(calibrator.isReady());
8560         assertFalse(calibrator.isRunning());
8561         assertNull(calibrator.getEstimatedBiases());
8562         assertFalse(calibrator.getEstimatedBiases(null));
8563         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8564         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8565         assertNull(calibrator.getEstimatedBiasFx());
8566         assertNull(calibrator.getEstimatedBiasFy());
8567         assertNull(calibrator.getEstimatedBiasFz());
8568         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8569         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8570         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8571         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8572         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8573         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8574         assertNull(calibrator.getEstimatedMa());
8575         assertNull(calibrator.getEstimatedSx());
8576         assertNull(calibrator.getEstimatedSy());
8577         assertNull(calibrator.getEstimatedSz());
8578         assertNull(calibrator.getEstimatedMxy());
8579         assertNull(calibrator.getEstimatedMxz());
8580         assertNull(calibrator.getEstimatedMyx());
8581         assertNull(calibrator.getEstimatedMyz());
8582         assertNull(calibrator.getEstimatedMzx());
8583         assertNull(calibrator.getEstimatedMzy());
8584         assertNull(calibrator.getEstimatedCovariance());
8585         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8586 
8587         // Force IllegalArgumentException
8588         calibrator = null;
8589         try {
8590             calibrator = new KnownPositionAccelerometerCalibrator(
8591                     true, new Matrix(1, 1), ma,
8592                     this);
8593             fail("IllegalArgumentException expected but not thrown");
8594         } catch (final IllegalArgumentException ignore) {
8595         }
8596         try {
8597             calibrator = new KnownPositionAccelerometerCalibrator(
8598                     true, new Matrix(1, 3), ma,
8599                     this);
8600             fail("IllegalArgumentException expected but not thrown");
8601         } catch (final IllegalArgumentException ignore) {
8602         }
8603         try {
8604             calibrator = new KnownPositionAccelerometerCalibrator(
8605                     true, ba, new Matrix(1, 3),
8606                     this);
8607             fail("IllegalArgumentException expected but not thrown");
8608         } catch (final IllegalArgumentException ignore) {
8609         }
8610         try {
8611             calibrator = new KnownPositionAccelerometerCalibrator(
8612                     true, ba, new Matrix(3, 1),
8613                     this);
8614             fail("IllegalArgumentException expected but not thrown");
8615         } catch (final IllegalArgumentException ignore) {
8616         }
8617         assertNull(calibrator);
8618     }
8619 
8620     @Test
8621     public void testConstructor77() throws WrongSizeException {
8622         final Collection<StandardDeviationBodyKinematics> measurements =
8623                 Collections.emptyList();
8624 
8625         final Matrix ba = generateBa();
8626         final double biasX = ba.getElementAtIndex(0);
8627         final double biasY = ba.getElementAtIndex(1);
8628         final double biasZ = ba.getElementAtIndex(2);
8629 
8630         final Matrix ma = generateMaCommonAxis();
8631         final double sx = ma.getElementAt(0, 0);
8632         final double sy = ma.getElementAt(1, 1);
8633         final double sz = ma.getElementAt(2, 2);
8634         final double mxy = ma.getElementAt(0, 1);
8635         final double mxz = ma.getElementAt(0, 2);
8636         final double myx = ma.getElementAt(1, 0);
8637         final double myz = ma.getElementAt(1, 2);
8638         final double mzx = ma.getElementAt(2, 0);
8639         final double mzy = ma.getElementAt(2, 1);
8640 
8641         KnownPositionAccelerometerCalibrator calibrator =
8642                 new KnownPositionAccelerometerCalibrator(measurements,
8643                         true, ba, ma);
8644 
8645         // check default values
8646         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8647         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8648         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8649         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8650         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8651         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8652         final Acceleration bx2 = new Acceleration(0.0,
8653                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8654         calibrator.getInitialBiasXAsAcceleration(bx2);
8655         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8656         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8657         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8658         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8659         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8660         final Acceleration by2 = new Acceleration(0.0,
8661                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8662         calibrator.getInitialBiasYAsAcceleration(by2);
8663         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8664         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8665         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8666         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8667         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8668         final Acceleration bz2 = new Acceleration(0.0,
8669                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8670         calibrator.getInitialBiasZAsAcceleration(bz2);
8671         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8672         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8673         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8674         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8675         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8676         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8677         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8678         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8679         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8680         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8681         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8682         final double[] bias1 = calibrator.getInitialBias();
8683         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8684         final double[] bias2 = new double[3];
8685         calibrator.getInitialBias(bias2);
8686         assertArrayEquals(bias1, bias2, 0.0);
8687         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8688         assertEquals(b1, ba);
8689         final Matrix b2 = new Matrix(3, 1);
8690         calibrator.getInitialBiasAsMatrix(b2);
8691         assertEquals(b1, b2);
8692         final Matrix ma1 = new Matrix(3, 3);
8693         ma1.setSubmatrix(0, 0,
8694                 2, 2,
8695                 new double[]{ sx, myx, mzx,
8696                         mxy, sy, mzy,
8697                         mxz, myz, sz});
8698         assertEquals(calibrator.getInitialMa(), ma1);
8699         final Matrix ma2 = new Matrix(3, 3);
8700         calibrator.getInitialMa(ma2);
8701         assertEquals(ma1, ma2);
8702         assertNull(calibrator.getEcefPosition());
8703         assertNull(calibrator.getNedPosition());
8704         assertFalse(calibrator.getNedPosition(null));
8705         assertSame(calibrator.getMeasurements(), measurements);
8706         assertTrue(calibrator.isCommonAxisUsed());
8707         assertNull(calibrator.getListener());
8708         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8709         assertFalse(calibrator.isReady());
8710         assertFalse(calibrator.isRunning());
8711         assertNull(calibrator.getEstimatedBiases());
8712         assertFalse(calibrator.getEstimatedBiases(null));
8713         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8714         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8715         assertNull(calibrator.getEstimatedBiasFx());
8716         assertNull(calibrator.getEstimatedBiasFy());
8717         assertNull(calibrator.getEstimatedBiasFz());
8718         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8719         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8720         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8721         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8722         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8723         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8724         assertNull(calibrator.getEstimatedMa());
8725         assertNull(calibrator.getEstimatedSx());
8726         assertNull(calibrator.getEstimatedSy());
8727         assertNull(calibrator.getEstimatedSz());
8728         assertNull(calibrator.getEstimatedMxy());
8729         assertNull(calibrator.getEstimatedMxz());
8730         assertNull(calibrator.getEstimatedMyx());
8731         assertNull(calibrator.getEstimatedMyz());
8732         assertNull(calibrator.getEstimatedMzx());
8733         assertNull(calibrator.getEstimatedMzy());
8734         assertNull(calibrator.getEstimatedCovariance());
8735         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8736 
8737         // Force IllegalArgumentException
8738         calibrator = null;
8739         try {
8740             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8741                     true, new Matrix(1, 1), ma);
8742             fail("IllegalArgumentException expected but not thrown");
8743         } catch (final IllegalArgumentException ignore) {
8744         }
8745         try {
8746             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8747                     true, new Matrix(1, 3), ma);
8748             fail("IllegalArgumentException expected but not thrown");
8749         } catch (final IllegalArgumentException ignore) {
8750         }
8751         try {
8752             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8753                     true, ba, new Matrix(1, 3));
8754             fail("IllegalArgumentException expected but not thrown");
8755         } catch (final IllegalArgumentException ignore) {
8756         }
8757         try {
8758             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8759                     true, ba, new Matrix(3, 1));
8760             fail("IllegalArgumentException expected but not thrown");
8761         } catch (final IllegalArgumentException ignore) {
8762         }
8763         assertNull(calibrator);
8764     }
8765 
8766     @Test
8767     public void testConstructor78() throws WrongSizeException {
8768         final Collection<StandardDeviationBodyKinematics> measurements =
8769                 Collections.emptyList();
8770 
8771         final Matrix ba = generateBa();
8772         final double biasX = ba.getElementAtIndex(0);
8773         final double biasY = ba.getElementAtIndex(1);
8774         final double biasZ = ba.getElementAtIndex(2);
8775 
8776         final Matrix ma = generateMaCommonAxis();
8777         final double sx = ma.getElementAt(0, 0);
8778         final double sy = ma.getElementAt(1, 1);
8779         final double sz = ma.getElementAt(2, 2);
8780         final double mxy = ma.getElementAt(0, 1);
8781         final double mxz = ma.getElementAt(0, 2);
8782         final double myx = ma.getElementAt(1, 0);
8783         final double myz = ma.getElementAt(1, 2);
8784         final double mzx = ma.getElementAt(2, 0);
8785         final double mzy = ma.getElementAt(2, 1);
8786 
8787         KnownPositionAccelerometerCalibrator calibrator =
8788                 new KnownPositionAccelerometerCalibrator(measurements,
8789                         true, ba, ma, this);
8790 
8791         // check default values
8792         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8793         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8794         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8795         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8796         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8797         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8798         final Acceleration bx2 = new Acceleration(0.0,
8799                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8800         calibrator.getInitialBiasXAsAcceleration(bx2);
8801         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8802         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8803         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8804         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8805         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8806         final Acceleration by2 = new Acceleration(0.0,
8807                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8808         calibrator.getInitialBiasYAsAcceleration(by2);
8809         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8810         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8811         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8812         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8813         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8814         final Acceleration bz2 = new Acceleration(0.0,
8815                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8816         calibrator.getInitialBiasZAsAcceleration(bz2);
8817         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8818         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8819         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8820         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8821         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8822         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8823         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8824         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8825         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8826         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8827         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8828         final double[] bias1 = calibrator.getInitialBias();
8829         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
8830         final double[] bias2 = new double[3];
8831         calibrator.getInitialBias(bias2);
8832         assertArrayEquals(bias1, bias2, 0.0);
8833         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8834         assertEquals(b1, ba);
8835         final Matrix b2 = new Matrix(3, 1);
8836         calibrator.getInitialBiasAsMatrix(b2);
8837         assertEquals(b1, b2);
8838         final Matrix ma1 = new Matrix(3, 3);
8839         ma1.setSubmatrix(0, 0,
8840                 2, 2,
8841                 new double[]{ sx, myx, mzx,
8842                         mxy, sy, mzy,
8843                         mxz, myz, sz});
8844         assertEquals(calibrator.getInitialMa(), ma1);
8845         final Matrix ma2 = new Matrix(3, 3);
8846         calibrator.getInitialMa(ma2);
8847         assertEquals(ma1, ma2);
8848         assertNull(calibrator.getEcefPosition());
8849         assertNull(calibrator.getNedPosition());
8850         assertFalse(calibrator.getNedPosition(null));
8851         assertSame(calibrator.getMeasurements(), measurements);
8852         assertTrue(calibrator.isCommonAxisUsed());
8853         assertSame(calibrator.getListener(), this);
8854         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8855         assertFalse(calibrator.isReady());
8856         assertFalse(calibrator.isRunning());
8857         assertNull(calibrator.getEstimatedBiases());
8858         assertFalse(calibrator.getEstimatedBiases(null));
8859         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8860         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8861         assertNull(calibrator.getEstimatedBiasFx());
8862         assertNull(calibrator.getEstimatedBiasFy());
8863         assertNull(calibrator.getEstimatedBiasFz());
8864         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8865         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8866         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8867         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8868         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8869         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8870         assertNull(calibrator.getEstimatedMa());
8871         assertNull(calibrator.getEstimatedSx());
8872         assertNull(calibrator.getEstimatedSy());
8873         assertNull(calibrator.getEstimatedSz());
8874         assertNull(calibrator.getEstimatedMxy());
8875         assertNull(calibrator.getEstimatedMxz());
8876         assertNull(calibrator.getEstimatedMyx());
8877         assertNull(calibrator.getEstimatedMyz());
8878         assertNull(calibrator.getEstimatedMzx());
8879         assertNull(calibrator.getEstimatedMzy());
8880         assertNull(calibrator.getEstimatedCovariance());
8881         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8882 
8883         // Force IllegalArgumentException
8884         calibrator = null;
8885         try {
8886             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8887                     true, new Matrix(1, 1), ma,
8888                     this);
8889             fail("IllegalArgumentException expected but not thrown");
8890         } catch (final IllegalArgumentException ignore) {
8891         }
8892         try {
8893             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8894                     true, new Matrix(1, 3), ma,
8895                     this);
8896             fail("IllegalArgumentException expected but not thrown");
8897         } catch (final IllegalArgumentException ignore) {
8898         }
8899         try {
8900             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8901                     true, ba, new Matrix(1, 3),
8902                     this);
8903             fail("IllegalArgumentException expected but not thrown");
8904         } catch (final IllegalArgumentException ignore) {
8905         }
8906         try {
8907             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8908                     true, ba, new Matrix(3, 1),
8909                     this);
8910             fail("IllegalArgumentException expected but not thrown");
8911         } catch (final IllegalArgumentException ignore) {
8912         }
8913         assertNull(calibrator);
8914     }
8915 
8916     @Test
8917     public void testConstructor79() throws WrongSizeException {
8918         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8919         final double latitude = Math.toRadians(
8920                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
8921         final double longitude = Math.toRadians(
8922                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8923         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8924         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8925         final NEDVelocity nedVelocity = new NEDVelocity();
8926         final ECEFPosition ecefPosition = new ECEFPosition();
8927         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8928         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8929                 ecefPosition, ecefVelocity);
8930 
8931         final KnownPositionAccelerometerCalibrator calibrator =
8932                 new KnownPositionAccelerometerCalibrator(ecefPosition);
8933 
8934         // check default values
8935         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
8936         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
8937         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
8938         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8939         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8940         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8941         final Acceleration bx2 = new Acceleration(0.0,
8942                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8943         calibrator.getInitialBiasXAsAcceleration(bx2);
8944         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8945         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8946         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8947         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8948         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8949         final Acceleration by2 = new Acceleration(0.0,
8950                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8951         calibrator.getInitialBiasYAsAcceleration(by2);
8952         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8953         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8954         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8955         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8956         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8957         final Acceleration bz2 = new Acceleration(0.0,
8958                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8959         calibrator.getInitialBiasZAsAcceleration(bz2);
8960         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
8961         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8962         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
8963         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
8964         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
8965         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
8966         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
8967         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
8968         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
8969         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
8970         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
8971         final double[] bias1 = calibrator.getInitialBias();
8972         assertArrayEquals(bias1, new double[3], 0.0);
8973         final double[] bias2 = new double[3];
8974         calibrator.getInitialBias(bias2);
8975         assertArrayEquals(bias1, bias2, 0.0);
8976         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8977         assertEquals(b1, new Matrix(3, 1));
8978         final Matrix b2 = new Matrix(3, 1);
8979         calibrator.getInitialBiasAsMatrix(b2);
8980         assertEquals(b1, b2);
8981         final Matrix ma1 = calibrator.getInitialMa();
8982         assertEquals(ma1, new Matrix(3, 3));
8983         final Matrix ma2 = new Matrix(3, 3);
8984         calibrator.getInitialMa(ma2);
8985         assertEquals(ma1, ma2);
8986         assertSame(calibrator.getEcefPosition(), ecefPosition);
8987         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
8988         final NEDPosition nedPosition1 = new NEDPosition();
8989         assertTrue(calibrator.getNedPosition(nedPosition1));
8990         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
8991         assertNull(calibrator.getMeasurements());
8992         assertFalse(calibrator.isCommonAxisUsed());
8993         assertNull(calibrator.getListener());
8994         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8995         assertFalse(calibrator.isReady());
8996         assertFalse(calibrator.isRunning());
8997         assertNull(calibrator.getEstimatedBiases());
8998         assertFalse(calibrator.getEstimatedBiases(null));
8999         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9000         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9001         assertNull(calibrator.getEstimatedBiasFx());
9002         assertNull(calibrator.getEstimatedBiasFy());
9003         assertNull(calibrator.getEstimatedBiasFz());
9004         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9005         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9006         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9007         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9008         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9009         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9010         assertNull(calibrator.getEstimatedMa());
9011         assertNull(calibrator.getEstimatedSx());
9012         assertNull(calibrator.getEstimatedSy());
9013         assertNull(calibrator.getEstimatedSz());
9014         assertNull(calibrator.getEstimatedMxy());
9015         assertNull(calibrator.getEstimatedMxz());
9016         assertNull(calibrator.getEstimatedMyx());
9017         assertNull(calibrator.getEstimatedMyz());
9018         assertNull(calibrator.getEstimatedMzx());
9019         assertNull(calibrator.getEstimatedMzy());
9020         assertNull(calibrator.getEstimatedCovariance());
9021         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9022     }
9023 
9024     @Test
9025     public void testConstructor80() throws WrongSizeException {
9026         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9027         final double latitude = Math.toRadians(
9028                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9029         final double longitude = Math.toRadians(
9030                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9031         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9032         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9033         final NEDVelocity nedVelocity = new NEDVelocity();
9034         final ECEFPosition ecefPosition = new ECEFPosition();
9035         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9036         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9037                 ecefPosition, ecefVelocity);
9038 
9039         final KnownPositionAccelerometerCalibrator calibrator =
9040                 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9041 
9042         // check default values
9043         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9044         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9045         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9046         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9047         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9048         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9049         final Acceleration bx2 = new Acceleration(0.0,
9050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9051         calibrator.getInitialBiasXAsAcceleration(bx2);
9052         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9053         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9054         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9055         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9056         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9057         final Acceleration by2 = new Acceleration(0.0,
9058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9059         calibrator.getInitialBiasYAsAcceleration(by2);
9060         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9061         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9062         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9063         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9064         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9065         final Acceleration bz2 = new Acceleration(0.0,
9066                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9067         calibrator.getInitialBiasZAsAcceleration(bz2);
9068         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9069         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9070         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9071         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9072         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9073         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9074         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9075         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9076         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9077         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9078         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9079         final double[] bias1 = calibrator.getInitialBias();
9080         assertArrayEquals(bias1, new double[3], 0.0);
9081         final double[] bias2 = new double[3];
9082         calibrator.getInitialBias(bias2);
9083         assertArrayEquals(bias1, bias2, 0.0);
9084         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9085         assertEquals(b1, new Matrix(3, 1));
9086         final Matrix b2 = new Matrix(3, 1);
9087         calibrator.getInitialBiasAsMatrix(b2);
9088         assertEquals(b1, b2);
9089         final Matrix ma1 = calibrator.getInitialMa();
9090         assertEquals(ma1, new Matrix(3, 3));
9091         final Matrix ma2 = new Matrix(3, 3);
9092         calibrator.getInitialMa(ma2);
9093         assertEquals(ma1, ma2);
9094         assertSame(calibrator.getEcefPosition(), ecefPosition);
9095         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9096         final NEDPosition nedPosition1 = new NEDPosition();
9097         assertTrue(calibrator.getNedPosition(nedPosition1));
9098         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9099         assertNull(calibrator.getMeasurements());
9100         assertFalse(calibrator.isCommonAxisUsed());
9101         assertSame(calibrator.getListener(), this);
9102         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9103         assertFalse(calibrator.isReady());
9104         assertFalse(calibrator.isRunning());
9105         assertNull(calibrator.getEstimatedBiases());
9106         assertFalse(calibrator.getEstimatedBiases(null));
9107         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9108         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9109         assertNull(calibrator.getEstimatedBiasFx());
9110         assertNull(calibrator.getEstimatedBiasFy());
9111         assertNull(calibrator.getEstimatedBiasFz());
9112         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9113         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9114         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9115         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9116         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9117         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9118         assertNull(calibrator.getEstimatedMa());
9119         assertNull(calibrator.getEstimatedSx());
9120         assertNull(calibrator.getEstimatedSy());
9121         assertNull(calibrator.getEstimatedSz());
9122         assertNull(calibrator.getEstimatedMxy());
9123         assertNull(calibrator.getEstimatedMxz());
9124         assertNull(calibrator.getEstimatedMyx());
9125         assertNull(calibrator.getEstimatedMyz());
9126         assertNull(calibrator.getEstimatedMzx());
9127         assertNull(calibrator.getEstimatedMzy());
9128         assertNull(calibrator.getEstimatedCovariance());
9129         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9130     }
9131 
9132     @Test
9133     public void testConstructor81() throws WrongSizeException {
9134         final Collection<StandardDeviationBodyKinematics> measurements =
9135                 Collections.emptyList();
9136 
9137         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9138         final double latitude = Math.toRadians(
9139                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9140         final double longitude = Math.toRadians(
9141                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9142         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9143         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9144         final NEDVelocity nedVelocity = new NEDVelocity();
9145         final ECEFPosition ecefPosition = new ECEFPosition();
9146         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9147         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9148                 ecefPosition, ecefVelocity);
9149 
9150         final KnownPositionAccelerometerCalibrator calibrator =
9151                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9152 
9153         // check default values
9154         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9155         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9156         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9157         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9158         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9159         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9160         final Acceleration bx2 = new Acceleration(0.0,
9161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9162         calibrator.getInitialBiasXAsAcceleration(bx2);
9163         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9164         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9165         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9166         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9167         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9168         final Acceleration by2 = new Acceleration(0.0,
9169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9170         calibrator.getInitialBiasYAsAcceleration(by2);
9171         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9172         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9173         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9174         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9175         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9176         final Acceleration bz2 = new Acceleration(0.0,
9177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9178         calibrator.getInitialBiasZAsAcceleration(bz2);
9179         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9180         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9181         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9182         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9183         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9184         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9185         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9186         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9187         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9188         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9189         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9190         final double[] bias1 = calibrator.getInitialBias();
9191         assertArrayEquals(bias1, new double[3], 0.0);
9192         final double[] bias2 = new double[3];
9193         calibrator.getInitialBias(bias2);
9194         assertArrayEquals(bias1, bias2, 0.0);
9195         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9196         assertEquals(b1, new Matrix(3, 1));
9197         final Matrix b2 = new Matrix(3, 1);
9198         calibrator.getInitialBiasAsMatrix(b2);
9199         assertEquals(b1, b2);
9200         final Matrix ma1 = calibrator.getInitialMa();
9201         assertEquals(ma1, new Matrix(3, 3));
9202         final Matrix ma2 = new Matrix(3, 3);
9203         calibrator.getInitialMa(ma2);
9204         assertEquals(ma1, ma2);
9205         assertSame(calibrator.getEcefPosition(), ecefPosition);
9206         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9207         final NEDPosition nedPosition1 = new NEDPosition();
9208         assertTrue(calibrator.getNedPosition(nedPosition1));
9209         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9210         assertSame(calibrator.getMeasurements(), measurements);
9211         assertFalse(calibrator.isCommonAxisUsed());
9212         assertNull(calibrator.getListener());
9213         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9214         assertFalse(calibrator.isReady());
9215         assertFalse(calibrator.isRunning());
9216         assertNull(calibrator.getEstimatedBiases());
9217         assertFalse(calibrator.getEstimatedBiases(null));
9218         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9219         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9220         assertNull(calibrator.getEstimatedBiasFx());
9221         assertNull(calibrator.getEstimatedBiasFy());
9222         assertNull(calibrator.getEstimatedBiasFz());
9223         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9224         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9225         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9226         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9227         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9228         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9229         assertNull(calibrator.getEstimatedMa());
9230         assertNull(calibrator.getEstimatedSx());
9231         assertNull(calibrator.getEstimatedSy());
9232         assertNull(calibrator.getEstimatedSz());
9233         assertNull(calibrator.getEstimatedMxy());
9234         assertNull(calibrator.getEstimatedMxz());
9235         assertNull(calibrator.getEstimatedMyx());
9236         assertNull(calibrator.getEstimatedMyz());
9237         assertNull(calibrator.getEstimatedMzx());
9238         assertNull(calibrator.getEstimatedMzy());
9239         assertNull(calibrator.getEstimatedCovariance());
9240         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9241     }
9242 
9243     @Test
9244     public void testConstructor82() throws WrongSizeException {
9245         final Collection<StandardDeviationBodyKinematics> measurements =
9246                 Collections.emptyList();
9247 
9248         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9249         final double latitude = Math.toRadians(
9250                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9251         final double longitude = Math.toRadians(
9252                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9253         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9254         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9255         final NEDVelocity nedVelocity = new NEDVelocity();
9256         final ECEFPosition ecefPosition = new ECEFPosition();
9257         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9258         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9259                 ecefPosition, ecefVelocity);
9260 
9261         final KnownPositionAccelerometerCalibrator calibrator =
9262                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9263                         this);
9264 
9265         // check default values
9266         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9267         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9268         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9269         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9270         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9271         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9272         final Acceleration bx2 = new Acceleration(0.0,
9273                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9274         calibrator.getInitialBiasXAsAcceleration(bx2);
9275         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9276         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9277         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9278         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9279         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9280         final Acceleration by2 = new Acceleration(0.0,
9281                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9282         calibrator.getInitialBiasYAsAcceleration(by2);
9283         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9284         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9285         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9286         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9287         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9288         final Acceleration bz2 = new Acceleration(0.0,
9289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9290         calibrator.getInitialBiasZAsAcceleration(bz2);
9291         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9292         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9293         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9294         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9295         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9296         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9297         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9298         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9299         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9300         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9301         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9302         final double[] bias1 = calibrator.getInitialBias();
9303         assertArrayEquals(bias1, new double[3], 0.0);
9304         final double[] bias2 = new double[3];
9305         calibrator.getInitialBias(bias2);
9306         assertArrayEquals(bias1, bias2, 0.0);
9307         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9308         assertEquals(b1, new Matrix(3, 1));
9309         final Matrix b2 = new Matrix(3, 1);
9310         calibrator.getInitialBiasAsMatrix(b2);
9311         assertEquals(b1, b2);
9312         final Matrix ma1 = calibrator.getInitialMa();
9313         assertEquals(ma1, new Matrix(3, 3));
9314         final Matrix ma2 = new Matrix(3, 3);
9315         calibrator.getInitialMa(ma2);
9316         assertEquals(ma1, ma2);
9317         assertSame(calibrator.getEcefPosition(), ecefPosition);
9318         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9319         final NEDPosition nedPosition1 = new NEDPosition();
9320         assertTrue(calibrator.getNedPosition(nedPosition1));
9321         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9322         assertSame(calibrator.getMeasurements(), measurements);
9323         assertFalse(calibrator.isCommonAxisUsed());
9324         assertSame(calibrator.getListener(), this);
9325         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9326         assertFalse(calibrator.isReady());
9327         assertFalse(calibrator.isRunning());
9328         assertNull(calibrator.getEstimatedBiases());
9329         assertFalse(calibrator.getEstimatedBiases(null));
9330         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9331         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9332         assertNull(calibrator.getEstimatedBiasFx());
9333         assertNull(calibrator.getEstimatedBiasFy());
9334         assertNull(calibrator.getEstimatedBiasFz());
9335         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9336         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9337         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9338         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9339         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9340         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9341         assertNull(calibrator.getEstimatedMa());
9342         assertNull(calibrator.getEstimatedSx());
9343         assertNull(calibrator.getEstimatedSy());
9344         assertNull(calibrator.getEstimatedSz());
9345         assertNull(calibrator.getEstimatedMxy());
9346         assertNull(calibrator.getEstimatedMxz());
9347         assertNull(calibrator.getEstimatedMyx());
9348         assertNull(calibrator.getEstimatedMyz());
9349         assertNull(calibrator.getEstimatedMzx());
9350         assertNull(calibrator.getEstimatedMzy());
9351         assertNull(calibrator.getEstimatedCovariance());
9352         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9353     }
9354 
9355     @Test
9356     public void testConstructor83() throws WrongSizeException {
9357         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9358         final double latitude = Math.toRadians(
9359                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9360         final double longitude = Math.toRadians(
9361                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9362         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9363         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9364         final NEDVelocity nedVelocity = new NEDVelocity();
9365         final ECEFPosition ecefPosition = new ECEFPosition();
9366         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9367         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9368                 ecefPosition, ecefVelocity);
9369 
9370         final KnownPositionAccelerometerCalibrator calibrator =
9371                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9372                         true);
9373 
9374         // check default values
9375         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9376         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9377         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9378         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9379         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9380         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9381         final Acceleration bx2 = new Acceleration(0.0,
9382                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9383         calibrator.getInitialBiasXAsAcceleration(bx2);
9384         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9385         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9386         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9387         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9388         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9389         final Acceleration by2 = new Acceleration(0.0,
9390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9391         calibrator.getInitialBiasYAsAcceleration(by2);
9392         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9393         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9394         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9395         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9396         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9397         final Acceleration bz2 = new Acceleration(0.0,
9398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9399         calibrator.getInitialBiasZAsAcceleration(bz2);
9400         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9401         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9402         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9403         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9404         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9405         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9406         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9407         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9408         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9409         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9410         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9411         final double[] bias1 = calibrator.getInitialBias();
9412         assertArrayEquals(bias1, new double[3], 0.0);
9413         final double[] bias2 = new double[3];
9414         calibrator.getInitialBias(bias2);
9415         assertArrayEquals(bias1, bias2, 0.0);
9416         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9417         assertEquals(b1, new Matrix(3, 1));
9418         final Matrix b2 = new Matrix(3, 1);
9419         calibrator.getInitialBiasAsMatrix(b2);
9420         assertEquals(b1, b2);
9421         final Matrix ma1 = calibrator.getInitialMa();
9422         assertEquals(ma1, new Matrix(3, 3));
9423         final Matrix ma2 = new Matrix(3, 3);
9424         calibrator.getInitialMa(ma2);
9425         assertEquals(ma1, ma2);
9426         assertSame(calibrator.getEcefPosition(), ecefPosition);
9427         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9428         final NEDPosition nedPosition1 = new NEDPosition();
9429         assertTrue(calibrator.getNedPosition(nedPosition1));
9430         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9431         assertNull(calibrator.getMeasurements());
9432         assertTrue(calibrator.isCommonAxisUsed());
9433         assertNull(calibrator.getListener());
9434         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9435         assertFalse(calibrator.isReady());
9436         assertFalse(calibrator.isRunning());
9437         assertNull(calibrator.getEstimatedBiases());
9438         assertFalse(calibrator.getEstimatedBiases(null));
9439         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9440         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9441         assertNull(calibrator.getEstimatedBiasFx());
9442         assertNull(calibrator.getEstimatedBiasFy());
9443         assertNull(calibrator.getEstimatedBiasFz());
9444         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9445         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9446         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9447         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9448         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9449         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9450         assertNull(calibrator.getEstimatedMa());
9451         assertNull(calibrator.getEstimatedSx());
9452         assertNull(calibrator.getEstimatedSy());
9453         assertNull(calibrator.getEstimatedSz());
9454         assertNull(calibrator.getEstimatedMxy());
9455         assertNull(calibrator.getEstimatedMxz());
9456         assertNull(calibrator.getEstimatedMyx());
9457         assertNull(calibrator.getEstimatedMyz());
9458         assertNull(calibrator.getEstimatedMzx());
9459         assertNull(calibrator.getEstimatedMzy());
9460         assertNull(calibrator.getEstimatedCovariance());
9461         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9462     }
9463 
9464     @Test
9465     public void testConstructor84() throws WrongSizeException {
9466         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9467         final double latitude = Math.toRadians(
9468                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9469         final double longitude = Math.toRadians(
9470                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9471         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9472         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9473         final NEDVelocity nedVelocity = new NEDVelocity();
9474         final ECEFPosition ecefPosition = new ECEFPosition();
9475         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9476         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9477                 ecefPosition, ecefVelocity);
9478 
9479         final KnownPositionAccelerometerCalibrator calibrator =
9480                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9481                         true, this);
9482 
9483         // check default values
9484         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9485         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9486         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9487         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9488         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9489         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9490         final Acceleration bx2 = new Acceleration(0.0,
9491                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9492         calibrator.getInitialBiasXAsAcceleration(bx2);
9493         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9494         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9495         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9496         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9497         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9498         final Acceleration by2 = new Acceleration(0.0,
9499                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9500         calibrator.getInitialBiasYAsAcceleration(by2);
9501         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9502         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9503         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9504         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9505         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9506         final Acceleration bz2 = new Acceleration(0.0,
9507                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9508         calibrator.getInitialBiasZAsAcceleration(bz2);
9509         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9510         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9511         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9512         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9513         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9514         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9515         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9516         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9517         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9518         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9519         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9520         final double[] bias1 = calibrator.getInitialBias();
9521         assertArrayEquals(bias1, new double[3], 0.0);
9522         final double[] bias2 = new double[3];
9523         calibrator.getInitialBias(bias2);
9524         assertArrayEquals(bias1, bias2, 0.0);
9525         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9526         assertEquals(b1, new Matrix(3, 1));
9527         final Matrix b2 = new Matrix(3, 1);
9528         calibrator.getInitialBiasAsMatrix(b2);
9529         assertEquals(b1, b2);
9530         final Matrix ma1 = calibrator.getInitialMa();
9531         assertEquals(ma1, new Matrix(3, 3));
9532         final Matrix ma2 = new Matrix(3, 3);
9533         calibrator.getInitialMa(ma2);
9534         assertEquals(ma1, ma2);
9535         assertSame(calibrator.getEcefPosition(), ecefPosition);
9536         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9537         final NEDPosition nedPosition1 = new NEDPosition();
9538         assertTrue(calibrator.getNedPosition(nedPosition1));
9539         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9540         assertNull(calibrator.getMeasurements());
9541         assertTrue(calibrator.isCommonAxisUsed());
9542         assertSame(calibrator.getListener(), this);
9543         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9544         assertFalse(calibrator.isReady());
9545         assertFalse(calibrator.isRunning());
9546         assertNull(calibrator.getEstimatedBiases());
9547         assertFalse(calibrator.getEstimatedBiases(null));
9548         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9549         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9550         assertNull(calibrator.getEstimatedBiasFx());
9551         assertNull(calibrator.getEstimatedBiasFy());
9552         assertNull(calibrator.getEstimatedBiasFz());
9553         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9554         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9555         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9556         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9557         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9558         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9559         assertNull(calibrator.getEstimatedMa());
9560         assertNull(calibrator.getEstimatedSx());
9561         assertNull(calibrator.getEstimatedSy());
9562         assertNull(calibrator.getEstimatedSz());
9563         assertNull(calibrator.getEstimatedMxy());
9564         assertNull(calibrator.getEstimatedMxz());
9565         assertNull(calibrator.getEstimatedMyx());
9566         assertNull(calibrator.getEstimatedMyz());
9567         assertNull(calibrator.getEstimatedMzx());
9568         assertNull(calibrator.getEstimatedMzy());
9569         assertNull(calibrator.getEstimatedCovariance());
9570         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9571     }
9572 
9573     @Test
9574     public void testConstructor85() throws WrongSizeException {
9575         final Collection<StandardDeviationBodyKinematics> measurements =
9576                 Collections.emptyList();
9577         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9578         final double latitude = Math.toRadians(
9579                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9580         final double longitude = Math.toRadians(
9581                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9582         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9583         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9584         final NEDVelocity nedVelocity = new NEDVelocity();
9585         final ECEFPosition ecefPosition = new ECEFPosition();
9586         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9587         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9588                 ecefPosition, ecefVelocity);
9589 
9590         final KnownPositionAccelerometerCalibrator calibrator =
9591                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9592                         true);
9593 
9594         // check default values
9595         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9596         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9597         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9598         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9599         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9600         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9601         final Acceleration bx2 = new Acceleration(0.0,
9602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9603         calibrator.getInitialBiasXAsAcceleration(bx2);
9604         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9605         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9606         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9607         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9608         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9609         final Acceleration by2 = new Acceleration(0.0,
9610                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9611         calibrator.getInitialBiasYAsAcceleration(by2);
9612         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9613         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9614         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9615         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9616         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9617         final Acceleration bz2 = new Acceleration(0.0,
9618                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9619         calibrator.getInitialBiasZAsAcceleration(bz2);
9620         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9621         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9622         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9623         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9624         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9625         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9626         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9627         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9628         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9629         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9630         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9631         final double[] bias1 = calibrator.getInitialBias();
9632         assertArrayEquals(bias1, new double[3], 0.0);
9633         final double[] bias2 = new double[3];
9634         calibrator.getInitialBias(bias2);
9635         assertArrayEquals(bias1, bias2, 0.0);
9636         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9637         assertEquals(b1, new Matrix(3, 1));
9638         final Matrix b2 = new Matrix(3, 1);
9639         calibrator.getInitialBiasAsMatrix(b2);
9640         assertEquals(b1, b2);
9641         final Matrix ma1 = calibrator.getInitialMa();
9642         assertEquals(ma1, new Matrix(3, 3));
9643         final Matrix ma2 = new Matrix(3, 3);
9644         calibrator.getInitialMa(ma2);
9645         assertEquals(ma1, ma2);
9646         assertSame(calibrator.getEcefPosition(), ecefPosition);
9647         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9648         final NEDPosition nedPosition1 = new NEDPosition();
9649         assertTrue(calibrator.getNedPosition(nedPosition1));
9650         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9651         assertSame(calibrator.getMeasurements(), measurements);
9652         assertTrue(calibrator.isCommonAxisUsed());
9653         assertNull(calibrator.getListener());
9654         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9655         assertFalse(calibrator.isReady());
9656         assertFalse(calibrator.isRunning());
9657         assertNull(calibrator.getEstimatedBiases());
9658         assertFalse(calibrator.getEstimatedBiases(null));
9659         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9660         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9661         assertNull(calibrator.getEstimatedBiasFx());
9662         assertNull(calibrator.getEstimatedBiasFy());
9663         assertNull(calibrator.getEstimatedBiasFz());
9664         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9665         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9666         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9667         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9668         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9669         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9670         assertNull(calibrator.getEstimatedMa());
9671         assertNull(calibrator.getEstimatedSx());
9672         assertNull(calibrator.getEstimatedSy());
9673         assertNull(calibrator.getEstimatedSz());
9674         assertNull(calibrator.getEstimatedMxy());
9675         assertNull(calibrator.getEstimatedMxz());
9676         assertNull(calibrator.getEstimatedMyx());
9677         assertNull(calibrator.getEstimatedMyz());
9678         assertNull(calibrator.getEstimatedMzx());
9679         assertNull(calibrator.getEstimatedMzy());
9680         assertNull(calibrator.getEstimatedCovariance());
9681         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9682     }
9683 
9684     @Test
9685     public void testConstructor86() throws WrongSizeException {
9686         final Collection<StandardDeviationBodyKinematics> measurements =
9687                 Collections.emptyList();
9688         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9689         final double latitude = Math.toRadians(
9690                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9691         final double longitude = Math.toRadians(
9692                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9693         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9694         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9695         final NEDVelocity nedVelocity = new NEDVelocity();
9696         final ECEFPosition ecefPosition = new ECEFPosition();
9697         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9698         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9699                 ecefPosition, ecefVelocity);
9700 
9701         final KnownPositionAccelerometerCalibrator calibrator =
9702                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9703                         true, this);
9704 
9705         // check default values
9706         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9707         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9708         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9709         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9710         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9711         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9712         final Acceleration bx2 = new Acceleration(0.0,
9713                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9714         calibrator.getInitialBiasXAsAcceleration(bx2);
9715         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9716         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9717         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9718         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9719         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9720         final Acceleration by2 = new Acceleration(0.0,
9721                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9722         calibrator.getInitialBiasYAsAcceleration(by2);
9723         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9724         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9725         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9726         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9727         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9728         final Acceleration bz2 = new Acceleration(0.0,
9729                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9730         calibrator.getInitialBiasZAsAcceleration(bz2);
9731         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9732         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9733         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9734         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9735         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9736         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9737         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9738         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9739         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9740         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9741         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9742         final double[] bias1 = calibrator.getInitialBias();
9743         assertArrayEquals(bias1, new double[3], 0.0);
9744         final double[] bias2 = new double[3];
9745         calibrator.getInitialBias(bias2);
9746         assertArrayEquals(bias1, bias2, 0.0);
9747         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9748         assertEquals(b1, new Matrix(3, 1));
9749         final Matrix b2 = new Matrix(3, 1);
9750         calibrator.getInitialBiasAsMatrix(b2);
9751         assertEquals(b1, b2);
9752         final Matrix ma1 = calibrator.getInitialMa();
9753         assertEquals(ma1, new Matrix(3, 3));
9754         final Matrix ma2 = new Matrix(3, 3);
9755         calibrator.getInitialMa(ma2);
9756         assertEquals(ma1, ma2);
9757         assertSame(calibrator.getEcefPosition(), ecefPosition);
9758         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9759         final NEDPosition nedPosition1 = new NEDPosition();
9760         assertTrue(calibrator.getNedPosition(nedPosition1));
9761         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9762         assertSame(calibrator.getMeasurements(), measurements);
9763         assertTrue(calibrator.isCommonAxisUsed());
9764         assertSame(calibrator.getListener(), this);
9765         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9766         assertFalse(calibrator.isReady());
9767         assertFalse(calibrator.isRunning());
9768         assertNull(calibrator.getEstimatedBiases());
9769         assertFalse(calibrator.getEstimatedBiases(null));
9770         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9771         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9772         assertNull(calibrator.getEstimatedBiasFx());
9773         assertNull(calibrator.getEstimatedBiasFy());
9774         assertNull(calibrator.getEstimatedBiasFz());
9775         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9776         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9777         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9778         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9779         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9780         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9781         assertNull(calibrator.getEstimatedMa());
9782         assertNull(calibrator.getEstimatedSx());
9783         assertNull(calibrator.getEstimatedSy());
9784         assertNull(calibrator.getEstimatedSz());
9785         assertNull(calibrator.getEstimatedMxy());
9786         assertNull(calibrator.getEstimatedMxz());
9787         assertNull(calibrator.getEstimatedMyx());
9788         assertNull(calibrator.getEstimatedMyz());
9789         assertNull(calibrator.getEstimatedMzx());
9790         assertNull(calibrator.getEstimatedMzy());
9791         assertNull(calibrator.getEstimatedCovariance());
9792         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9793     }
9794 
9795     @Test
9796     public void testConstructor87() throws WrongSizeException {
9797         final Matrix ba = generateBa();
9798         final double biasX = ba.getElementAtIndex(0);
9799         final double biasY = ba.getElementAtIndex(1);
9800         final double biasZ = ba.getElementAtIndex(2);
9801 
9802         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9803         final double latitude = Math.toRadians(
9804                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9805         final double longitude = Math.toRadians(
9806                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9807         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9808         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9809         final NEDVelocity nedVelocity = new NEDVelocity();
9810         final ECEFPosition ecefPosition = new ECEFPosition();
9811         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9812         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9813                 ecefPosition, ecefVelocity);
9814 
9815         final KnownPositionAccelerometerCalibrator calibrator =
9816                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9817                         biasX, biasY, biasZ);
9818 
9819         // check default values
9820         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9821         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9822         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9823         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9824         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9825         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9826         final Acceleration bx2 = new Acceleration(0.0,
9827                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9828         calibrator.getInitialBiasXAsAcceleration(bx2);
9829         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9830         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9831         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9832         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9833         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9834         final Acceleration by2 = new Acceleration(0.0,
9835                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9836         calibrator.getInitialBiasYAsAcceleration(by2);
9837         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9838         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9839         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9840         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9841         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9842         final Acceleration bz2 = new Acceleration(0.0,
9843                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9844         calibrator.getInitialBiasZAsAcceleration(bz2);
9845         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9846         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9847         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9848         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9849         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9850         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9851         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9852         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9853         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9854         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9855         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9856         final double[] bias1 = calibrator.getInitialBias();
9857         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
9858         final double[] bias2 = new double[3];
9859         calibrator.getInitialBias(bias2);
9860         assertArrayEquals(bias1, bias2, 0.0);
9861         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9862         assertEquals(b1, ba);
9863         final Matrix b2 = new Matrix(3, 1);
9864         calibrator.getInitialBiasAsMatrix(b2);
9865         assertEquals(b1, b2);
9866         final Matrix ma1 = calibrator.getInitialMa();
9867         assertEquals(ma1, new Matrix(3, 3));
9868         final Matrix ma2 = new Matrix(3, 3);
9869         calibrator.getInitialMa(ma2);
9870         assertEquals(ma1, ma2);
9871         assertSame(calibrator.getEcefPosition(), ecefPosition);
9872         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9873         final NEDPosition nedPosition1 = new NEDPosition();
9874         assertTrue(calibrator.getNedPosition(nedPosition1));
9875         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9876         assertNull(calibrator.getMeasurements());
9877         assertFalse(calibrator.isCommonAxisUsed());
9878         assertNull(calibrator.getListener());
9879         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9880         assertFalse(calibrator.isReady());
9881         assertFalse(calibrator.isRunning());
9882         assertNull(calibrator.getEstimatedBiases());
9883         assertFalse(calibrator.getEstimatedBiases(null));
9884         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9885         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9886         assertNull(calibrator.getEstimatedBiasFx());
9887         assertNull(calibrator.getEstimatedBiasFy());
9888         assertNull(calibrator.getEstimatedBiasFz());
9889         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9890         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9891         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9892         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9893         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9894         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9895         assertNull(calibrator.getEstimatedMa());
9896         assertNull(calibrator.getEstimatedSx());
9897         assertNull(calibrator.getEstimatedSy());
9898         assertNull(calibrator.getEstimatedSz());
9899         assertNull(calibrator.getEstimatedMxy());
9900         assertNull(calibrator.getEstimatedMxz());
9901         assertNull(calibrator.getEstimatedMyx());
9902         assertNull(calibrator.getEstimatedMyz());
9903         assertNull(calibrator.getEstimatedMzx());
9904         assertNull(calibrator.getEstimatedMzy());
9905         assertNull(calibrator.getEstimatedCovariance());
9906         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9907     }
9908 
9909     @Test
9910     public void testConstructor88() throws WrongSizeException {
9911         final Matrix ba = generateBa();
9912         final double biasX = ba.getElementAtIndex(0);
9913         final double biasY = ba.getElementAtIndex(1);
9914         final double biasZ = ba.getElementAtIndex(2);
9915 
9916         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9917         final double latitude = Math.toRadians(
9918                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9919         final double longitude = Math.toRadians(
9920                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9921         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9922         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9923         final NEDVelocity nedVelocity = new NEDVelocity();
9924         final ECEFPosition ecefPosition = new ECEFPosition();
9925         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9926         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9927                 ecefPosition, ecefVelocity);
9928 
9929         final KnownPositionAccelerometerCalibrator calibrator =
9930                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9931                         biasX, biasY, biasZ, this);
9932 
9933         // check default values
9934         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9935         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9936         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9937         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9938         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9939         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9940         final Acceleration bx2 = new Acceleration(0.0,
9941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9942         calibrator.getInitialBiasXAsAcceleration(bx2);
9943         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9944         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9945         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9946         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9947         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9948         final Acceleration by2 = new Acceleration(0.0,
9949                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9950         calibrator.getInitialBiasYAsAcceleration(by2);
9951         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9952         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9953         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9954         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9955         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9956         final Acceleration bz2 = new Acceleration(0.0,
9957                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9958         calibrator.getInitialBiasZAsAcceleration(bz2);
9959         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9960         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9961         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9962         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9963         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9964         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9965         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9966         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9967         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9968         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9969         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9970         final double[] bias1 = calibrator.getInitialBias();
9971         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
9972         final double[] bias2 = new double[3];
9973         calibrator.getInitialBias(bias2);
9974         assertArrayEquals(bias1, bias2, 0.0);
9975         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9976         assertEquals(b1, ba);
9977         final Matrix b2 = new Matrix(3, 1);
9978         calibrator.getInitialBiasAsMatrix(b2);
9979         assertEquals(b1, b2);
9980         final Matrix ma1 = calibrator.getInitialMa();
9981         assertEquals(ma1, new Matrix(3, 3));
9982         final Matrix ma2 = new Matrix(3, 3);
9983         calibrator.getInitialMa(ma2);
9984         assertEquals(ma1, ma2);
9985         assertSame(calibrator.getEcefPosition(), ecefPosition);
9986         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9987         final NEDPosition nedPosition1 = new NEDPosition();
9988         assertTrue(calibrator.getNedPosition(nedPosition1));
9989         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9990         assertNull(calibrator.getMeasurements());
9991         assertFalse(calibrator.isCommonAxisUsed());
9992         assertSame(calibrator.getListener(), this);
9993         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9994         assertFalse(calibrator.isReady());
9995         assertFalse(calibrator.isRunning());
9996         assertNull(calibrator.getEstimatedBiases());
9997         assertFalse(calibrator.getEstimatedBiases(null));
9998         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9999         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10000         assertNull(calibrator.getEstimatedBiasFx());
10001         assertNull(calibrator.getEstimatedBiasFy());
10002         assertNull(calibrator.getEstimatedBiasFz());
10003         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10004         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10005         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10006         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10007         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10008         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10009         assertNull(calibrator.getEstimatedMa());
10010         assertNull(calibrator.getEstimatedSx());
10011         assertNull(calibrator.getEstimatedSy());
10012         assertNull(calibrator.getEstimatedSz());
10013         assertNull(calibrator.getEstimatedMxy());
10014         assertNull(calibrator.getEstimatedMxz());
10015         assertNull(calibrator.getEstimatedMyx());
10016         assertNull(calibrator.getEstimatedMyz());
10017         assertNull(calibrator.getEstimatedMzx());
10018         assertNull(calibrator.getEstimatedMzy());
10019         assertNull(calibrator.getEstimatedCovariance());
10020         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10021     }
10022 
10023     @Test
10024     public void testConstructor89() throws WrongSizeException {
10025         final Collection<StandardDeviationBodyKinematics> measurements =
10026                 Collections.emptyList();
10027 
10028         final Matrix ba = generateBa();
10029         final double biasX = ba.getElementAtIndex(0);
10030         final double biasY = ba.getElementAtIndex(1);
10031         final double biasZ = ba.getElementAtIndex(2);
10032 
10033         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10034         final double latitude = Math.toRadians(
10035                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10036         final double longitude = Math.toRadians(
10037                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10038         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10039         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10040         final NEDVelocity nedVelocity = new NEDVelocity();
10041         final ECEFPosition ecefPosition = new ECEFPosition();
10042         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10043         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10044                 ecefPosition, ecefVelocity);
10045 
10046         final KnownPositionAccelerometerCalibrator calibrator =
10047                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10048                         biasX, biasY, biasZ);
10049 
10050         // check default values
10051         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10052         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10053         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10054         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10055         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10056         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10057         final Acceleration bx2 = new Acceleration(0.0,
10058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10059         calibrator.getInitialBiasXAsAcceleration(bx2);
10060         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10061         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10062         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10063         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10064         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10065         final Acceleration by2 = new Acceleration(0.0,
10066                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10067         calibrator.getInitialBiasYAsAcceleration(by2);
10068         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10069         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10070         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10071         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10072         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10073         final Acceleration bz2 = new Acceleration(0.0,
10074                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10075         calibrator.getInitialBiasZAsAcceleration(bz2);
10076         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10077         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10078         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10079         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10080         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10081         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10082         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10083         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10084         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10085         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10086         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10087         final double[] bias1 = calibrator.getInitialBias();
10088         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10089         final double[] bias2 = new double[3];
10090         calibrator.getInitialBias(bias2);
10091         assertArrayEquals(bias1, bias2, 0.0);
10092         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10093         assertEquals(b1, ba);
10094         final Matrix b2 = new Matrix(3, 1);
10095         calibrator.getInitialBiasAsMatrix(b2);
10096         assertEquals(b1, b2);
10097         final Matrix ma1 = calibrator.getInitialMa();
10098         assertEquals(ma1, new Matrix(3, 3));
10099         final Matrix ma2 = new Matrix(3, 3);
10100         calibrator.getInitialMa(ma2);
10101         assertEquals(ma1, ma2);
10102         assertSame(calibrator.getEcefPosition(), ecefPosition);
10103         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10104         final NEDPosition nedPosition1 = new NEDPosition();
10105         assertTrue(calibrator.getNedPosition(nedPosition1));
10106         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10107         assertSame(calibrator.getMeasurements(), measurements);
10108         assertFalse(calibrator.isCommonAxisUsed());
10109         assertNull(calibrator.getListener());
10110         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10111         assertFalse(calibrator.isReady());
10112         assertFalse(calibrator.isRunning());
10113         assertNull(calibrator.getEstimatedBiases());
10114         assertFalse(calibrator.getEstimatedBiases(null));
10115         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10116         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10117         assertNull(calibrator.getEstimatedBiasFx());
10118         assertNull(calibrator.getEstimatedBiasFy());
10119         assertNull(calibrator.getEstimatedBiasFz());
10120         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10121         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10122         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10123         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10124         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10125         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10126         assertNull(calibrator.getEstimatedMa());
10127         assertNull(calibrator.getEstimatedSx());
10128         assertNull(calibrator.getEstimatedSy());
10129         assertNull(calibrator.getEstimatedSz());
10130         assertNull(calibrator.getEstimatedMxy());
10131         assertNull(calibrator.getEstimatedMxz());
10132         assertNull(calibrator.getEstimatedMyx());
10133         assertNull(calibrator.getEstimatedMyz());
10134         assertNull(calibrator.getEstimatedMzx());
10135         assertNull(calibrator.getEstimatedMzy());
10136         assertNull(calibrator.getEstimatedCovariance());
10137         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10138     }
10139 
10140     @Test
10141     public void testConstructor90() throws WrongSizeException {
10142         final Collection<StandardDeviationBodyKinematics> measurements =
10143                 Collections.emptyList();
10144 
10145         final Matrix ba = generateBa();
10146         final double biasX = ba.getElementAtIndex(0);
10147         final double biasY = ba.getElementAtIndex(1);
10148         final double biasZ = ba.getElementAtIndex(2);
10149 
10150         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10151         final double latitude = Math.toRadians(
10152                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10153         final double longitude = Math.toRadians(
10154                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10155         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10156         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10157         final NEDVelocity nedVelocity = new NEDVelocity();
10158         final ECEFPosition ecefPosition = new ECEFPosition();
10159         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10160         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10161                 ecefPosition, ecefVelocity);
10162 
10163         final KnownPositionAccelerometerCalibrator calibrator =
10164                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10165                         biasX, biasY, biasZ, this);
10166 
10167         // check default values
10168         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10169         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10170         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10171         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10172         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10173         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10174         final Acceleration bx2 = new Acceleration(0.0,
10175                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10176         calibrator.getInitialBiasXAsAcceleration(bx2);
10177         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10178         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10179         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10180         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10181         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10182         final Acceleration by2 = new Acceleration(0.0,
10183                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10184         calibrator.getInitialBiasYAsAcceleration(by2);
10185         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10186         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10187         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10188         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10189         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10190         final Acceleration bz2 = new Acceleration(0.0,
10191                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10192         calibrator.getInitialBiasZAsAcceleration(bz2);
10193         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10194         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10195         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10196         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10197         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10198         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10199         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10200         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10201         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10202         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10203         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10204         final double[] bias1 = calibrator.getInitialBias();
10205         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10206         final double[] bias2 = new double[3];
10207         calibrator.getInitialBias(bias2);
10208         assertArrayEquals(bias1, bias2, 0.0);
10209         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10210         assertEquals(b1, ba);
10211         final Matrix b2 = new Matrix(3, 1);
10212         calibrator.getInitialBiasAsMatrix(b2);
10213         assertEquals(b1, b2);
10214         final Matrix ma1 = calibrator.getInitialMa();
10215         assertEquals(ma1, new Matrix(3, 3));
10216         final Matrix ma2 = new Matrix(3, 3);
10217         calibrator.getInitialMa(ma2);
10218         assertEquals(ma1, ma2);
10219         assertSame(calibrator.getEcefPosition(), ecefPosition);
10220         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10221         final NEDPosition nedPosition1 = new NEDPosition();
10222         assertTrue(calibrator.getNedPosition(nedPosition1));
10223         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10224         assertSame(calibrator.getMeasurements(), measurements);
10225         assertFalse(calibrator.isCommonAxisUsed());
10226         assertSame(calibrator.getListener(), this);
10227         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10228         assertFalse(calibrator.isReady());
10229         assertFalse(calibrator.isRunning());
10230         assertNull(calibrator.getEstimatedBiases());
10231         assertFalse(calibrator.getEstimatedBiases(null));
10232         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10233         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10234         assertNull(calibrator.getEstimatedBiasFx());
10235         assertNull(calibrator.getEstimatedBiasFy());
10236         assertNull(calibrator.getEstimatedBiasFz());
10237         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10238         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10239         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10240         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10241         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10242         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10243         assertNull(calibrator.getEstimatedMa());
10244         assertNull(calibrator.getEstimatedSx());
10245         assertNull(calibrator.getEstimatedSy());
10246         assertNull(calibrator.getEstimatedSz());
10247         assertNull(calibrator.getEstimatedMxy());
10248         assertNull(calibrator.getEstimatedMxz());
10249         assertNull(calibrator.getEstimatedMyx());
10250         assertNull(calibrator.getEstimatedMyz());
10251         assertNull(calibrator.getEstimatedMzx());
10252         assertNull(calibrator.getEstimatedMzy());
10253         assertNull(calibrator.getEstimatedCovariance());
10254         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10255     }
10256 
10257     @Test
10258     public void testConstructor91() throws WrongSizeException {
10259         final Matrix ba = generateBa();
10260         final double biasX = ba.getElementAtIndex(0);
10261         final double biasY = ba.getElementAtIndex(1);
10262         final double biasZ = ba.getElementAtIndex(2);
10263 
10264         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10265         final double latitude = Math.toRadians(
10266                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10267         final double longitude = Math.toRadians(
10268                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10269         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10270         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10271         final NEDVelocity nedVelocity = new NEDVelocity();
10272         final ECEFPosition ecefPosition = new ECEFPosition();
10273         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10274         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10275                 ecefPosition, ecefVelocity);
10276 
10277         final KnownPositionAccelerometerCalibrator calibrator =
10278                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10279                         true, biasX, biasY, biasZ);
10280 
10281         // check default values
10282         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10283         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10284         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10285         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10286         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10287         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10288         final Acceleration bx2 = new Acceleration(0.0,
10289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10290         calibrator.getInitialBiasXAsAcceleration(bx2);
10291         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10292         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10293         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10294         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10295         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10296         final Acceleration by2 = new Acceleration(0.0,
10297                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10298         calibrator.getInitialBiasYAsAcceleration(by2);
10299         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10300         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10301         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10302         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10303         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10304         final Acceleration bz2 = new Acceleration(0.0,
10305                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10306         calibrator.getInitialBiasZAsAcceleration(bz2);
10307         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10308         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10309         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10310         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10311         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10312         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10313         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10314         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10315         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10316         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10317         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10318         final double[] bias1 = calibrator.getInitialBias();
10319         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10320         final double[] bias2 = new double[3];
10321         calibrator.getInitialBias(bias2);
10322         assertArrayEquals(bias1, bias2, 0.0);
10323         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10324         assertEquals(b1, ba);
10325         final Matrix b2 = new Matrix(3, 1);
10326         calibrator.getInitialBiasAsMatrix(b2);
10327         assertEquals(b1, b2);
10328         final Matrix ma1 = calibrator.getInitialMa();
10329         assertEquals(ma1, new Matrix(3, 3));
10330         final Matrix ma2 = new Matrix(3, 3);
10331         calibrator.getInitialMa(ma2);
10332         assertEquals(ma1, ma2);
10333         assertSame(calibrator.getEcefPosition(), ecefPosition);
10334         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10335         final NEDPosition nedPosition1 = new NEDPosition();
10336         assertTrue(calibrator.getNedPosition(nedPosition1));
10337         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10338         assertNull(calibrator.getMeasurements());
10339         assertTrue(calibrator.isCommonAxisUsed());
10340         assertNull(calibrator.getListener());
10341         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10342         assertFalse(calibrator.isReady());
10343         assertFalse(calibrator.isRunning());
10344         assertNull(calibrator.getEstimatedBiases());
10345         assertFalse(calibrator.getEstimatedBiases(null));
10346         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10347         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10348         assertNull(calibrator.getEstimatedBiasFx());
10349         assertNull(calibrator.getEstimatedBiasFy());
10350         assertNull(calibrator.getEstimatedBiasFz());
10351         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10352         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10353         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10354         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10355         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10356         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10357         assertNull(calibrator.getEstimatedMa());
10358         assertNull(calibrator.getEstimatedSx());
10359         assertNull(calibrator.getEstimatedSy());
10360         assertNull(calibrator.getEstimatedSz());
10361         assertNull(calibrator.getEstimatedMxy());
10362         assertNull(calibrator.getEstimatedMxz());
10363         assertNull(calibrator.getEstimatedMyx());
10364         assertNull(calibrator.getEstimatedMyz());
10365         assertNull(calibrator.getEstimatedMzx());
10366         assertNull(calibrator.getEstimatedMzy());
10367         assertNull(calibrator.getEstimatedCovariance());
10368         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10369     }
10370 
10371     @Test
10372     public void testConstructor92() throws WrongSizeException {
10373         final Matrix ba = generateBa();
10374         final double biasX = ba.getElementAtIndex(0);
10375         final double biasY = ba.getElementAtIndex(1);
10376         final double biasZ = ba.getElementAtIndex(2);
10377 
10378         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10379         final double latitude = Math.toRadians(
10380                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10381         final double longitude = Math.toRadians(
10382                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10383         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10384         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10385         final NEDVelocity nedVelocity = new NEDVelocity();
10386         final ECEFPosition ecefPosition = new ECEFPosition();
10387         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10388         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10389                 ecefPosition, ecefVelocity);
10390 
10391         final KnownPositionAccelerometerCalibrator calibrator =
10392                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10393                         true, biasX, biasY, biasZ, this);
10394 
10395         // check default values
10396         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10397         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10398         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10399         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10400         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10401         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10402         final Acceleration bx2 = new Acceleration(0.0,
10403                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10404         calibrator.getInitialBiasXAsAcceleration(bx2);
10405         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10406         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10407         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10408         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10409         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10410         final Acceleration by2 = new Acceleration(0.0,
10411                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10412         calibrator.getInitialBiasYAsAcceleration(by2);
10413         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10414         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10415         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10416         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10417         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10418         final Acceleration bz2 = new Acceleration(0.0,
10419                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10420         calibrator.getInitialBiasZAsAcceleration(bz2);
10421         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10422         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10423         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10424         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10425         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10426         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10427         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10428         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10429         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10430         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10431         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10432         final double[] bias1 = calibrator.getInitialBias();
10433         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10434         final double[] bias2 = new double[3];
10435         calibrator.getInitialBias(bias2);
10436         assertArrayEquals(bias1, bias2, 0.0);
10437         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10438         assertEquals(b1, ba);
10439         final Matrix b2 = new Matrix(3, 1);
10440         calibrator.getInitialBiasAsMatrix(b2);
10441         assertEquals(b1, b2);
10442         final Matrix ma1 = calibrator.getInitialMa();
10443         assertEquals(ma1, new Matrix(3, 3));
10444         final Matrix ma2 = new Matrix(3, 3);
10445         calibrator.getInitialMa(ma2);
10446         assertEquals(ma1, ma2);
10447         assertSame(calibrator.getEcefPosition(), ecefPosition);
10448         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10449         final NEDPosition nedPosition1 = new NEDPosition();
10450         assertTrue(calibrator.getNedPosition(nedPosition1));
10451         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10452         assertNull(calibrator.getMeasurements());
10453         assertTrue(calibrator.isCommonAxisUsed());
10454         assertSame(calibrator.getListener(), this);
10455         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10456         assertFalse(calibrator.isReady());
10457         assertFalse(calibrator.isRunning());
10458         assertNull(calibrator.getEstimatedBiases());
10459         assertFalse(calibrator.getEstimatedBiases(null));
10460         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10461         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10462         assertNull(calibrator.getEstimatedBiasFx());
10463         assertNull(calibrator.getEstimatedBiasFy());
10464         assertNull(calibrator.getEstimatedBiasFz());
10465         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10466         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10467         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10468         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10469         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10470         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10471         assertNull(calibrator.getEstimatedMa());
10472         assertNull(calibrator.getEstimatedSx());
10473         assertNull(calibrator.getEstimatedSy());
10474         assertNull(calibrator.getEstimatedSz());
10475         assertNull(calibrator.getEstimatedMxy());
10476         assertNull(calibrator.getEstimatedMxz());
10477         assertNull(calibrator.getEstimatedMyx());
10478         assertNull(calibrator.getEstimatedMyz());
10479         assertNull(calibrator.getEstimatedMzx());
10480         assertNull(calibrator.getEstimatedMzy());
10481         assertNull(calibrator.getEstimatedCovariance());
10482         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10483     }
10484 
10485     @Test
10486     public void testConstructor93() throws WrongSizeException {
10487         final Collection<StandardDeviationBodyKinematics> measurements =
10488                 Collections.emptyList();
10489 
10490         final Matrix ba = generateBa();
10491         final double biasX = ba.getElementAtIndex(0);
10492         final double biasY = ba.getElementAtIndex(1);
10493         final double biasZ = ba.getElementAtIndex(2);
10494 
10495         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10496         final double latitude = Math.toRadians(
10497                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10498         final double longitude = Math.toRadians(
10499                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10500         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10501         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10502         final NEDVelocity nedVelocity = new NEDVelocity();
10503         final ECEFPosition ecefPosition = new ECEFPosition();
10504         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10505         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10506                 ecefPosition, ecefVelocity);
10507 
10508         final KnownPositionAccelerometerCalibrator calibrator =
10509                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10510                         true, biasX, biasY, biasZ);
10511 
10512         // check default values
10513         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10514         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10515         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10516         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10517         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10518         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10519         final Acceleration bx2 = new Acceleration(0.0,
10520                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10521         calibrator.getInitialBiasXAsAcceleration(bx2);
10522         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10523         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10524         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10525         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10526         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10527         final Acceleration by2 = new Acceleration(0.0,
10528                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10529         calibrator.getInitialBiasYAsAcceleration(by2);
10530         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10531         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10532         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10533         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10534         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10535         final Acceleration bz2 = new Acceleration(0.0,
10536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10537         calibrator.getInitialBiasZAsAcceleration(bz2);
10538         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10539         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10540         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10541         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10542         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10543         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10544         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10545         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10546         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10547         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10548         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10549         final double[] bias1 = calibrator.getInitialBias();
10550         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10551         final double[] bias2 = new double[3];
10552         calibrator.getInitialBias(bias2);
10553         assertArrayEquals(bias1, bias2, 0.0);
10554         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10555         assertEquals(b1, ba);
10556         final Matrix b2 = new Matrix(3, 1);
10557         calibrator.getInitialBiasAsMatrix(b2);
10558         assertEquals(b1, b2);
10559         final Matrix ma1 = calibrator.getInitialMa();
10560         assertEquals(ma1, new Matrix(3, 3));
10561         final Matrix ma2 = new Matrix(3, 3);
10562         calibrator.getInitialMa(ma2);
10563         assertEquals(ma1, ma2);
10564         assertSame(calibrator.getEcefPosition(), ecefPosition);
10565         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10566         final NEDPosition nedPosition1 = new NEDPosition();
10567         assertTrue(calibrator.getNedPosition(nedPosition1));
10568         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10569         assertSame(calibrator.getMeasurements(), measurements);
10570         assertTrue(calibrator.isCommonAxisUsed());
10571         assertNull(calibrator.getListener());
10572         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10573         assertFalse(calibrator.isReady());
10574         assertFalse(calibrator.isRunning());
10575         assertNull(calibrator.getEstimatedBiases());
10576         assertFalse(calibrator.getEstimatedBiases(null));
10577         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10578         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10579         assertNull(calibrator.getEstimatedBiasFx());
10580         assertNull(calibrator.getEstimatedBiasFy());
10581         assertNull(calibrator.getEstimatedBiasFz());
10582         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10583         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10584         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10585         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10586         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10587         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10588         assertNull(calibrator.getEstimatedMa());
10589         assertNull(calibrator.getEstimatedSx());
10590         assertNull(calibrator.getEstimatedSy());
10591         assertNull(calibrator.getEstimatedSz());
10592         assertNull(calibrator.getEstimatedMxy());
10593         assertNull(calibrator.getEstimatedMxz());
10594         assertNull(calibrator.getEstimatedMyx());
10595         assertNull(calibrator.getEstimatedMyz());
10596         assertNull(calibrator.getEstimatedMzx());
10597         assertNull(calibrator.getEstimatedMzy());
10598         assertNull(calibrator.getEstimatedCovariance());
10599         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10600     }
10601 
10602     @Test
10603     public void testConstructor94() throws WrongSizeException {
10604         final Collection<StandardDeviationBodyKinematics> measurements =
10605                 Collections.emptyList();
10606 
10607         final Matrix ba = generateBa();
10608         final double biasX = ba.getElementAtIndex(0);
10609         final double biasY = ba.getElementAtIndex(1);
10610         final double biasZ = ba.getElementAtIndex(2);
10611 
10612         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10613         final double latitude = Math.toRadians(
10614                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10615         final double longitude = Math.toRadians(
10616                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10617         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10618         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10619         final NEDVelocity nedVelocity = new NEDVelocity();
10620         final ECEFPosition ecefPosition = new ECEFPosition();
10621         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10622         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10623                 ecefPosition, ecefVelocity);
10624 
10625         final KnownPositionAccelerometerCalibrator calibrator =
10626                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10627                         true, biasX, biasY, biasZ, this);
10628 
10629         // check default values
10630         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10631         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10632         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10633         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10634         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10635         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10636         final Acceleration bx2 = new Acceleration(0.0,
10637                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10638         calibrator.getInitialBiasXAsAcceleration(bx2);
10639         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10640         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10641         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10642         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10643         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10644         final Acceleration by2 = new Acceleration(0.0,
10645                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10646         calibrator.getInitialBiasYAsAcceleration(by2);
10647         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10648         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10649         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10650         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10651         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10652         final Acceleration bz2 = new Acceleration(0.0,
10653                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10654         calibrator.getInitialBiasZAsAcceleration(bz2);
10655         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10656         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10657         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10658         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10659         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10660         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10661         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10662         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10663         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10664         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10665         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10666         final double[] bias1 = calibrator.getInitialBias();
10667         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10668         final double[] bias2 = new double[3];
10669         calibrator.getInitialBias(bias2);
10670         assertArrayEquals(bias1, bias2, 0.0);
10671         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10672         assertEquals(b1, ba);
10673         final Matrix b2 = new Matrix(3, 1);
10674         calibrator.getInitialBiasAsMatrix(b2);
10675         assertEquals(b1, b2);
10676         final Matrix ma1 = calibrator.getInitialMa();
10677         assertEquals(ma1, new Matrix(3, 3));
10678         final Matrix ma2 = new Matrix(3, 3);
10679         calibrator.getInitialMa(ma2);
10680         assertEquals(ma1, ma2);
10681         assertSame(calibrator.getEcefPosition(), ecefPosition);
10682         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10683         final NEDPosition nedPosition1 = new NEDPosition();
10684         assertTrue(calibrator.getNedPosition(nedPosition1));
10685         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10686         assertSame(calibrator.getMeasurements(), measurements);
10687         assertTrue(calibrator.isCommonAxisUsed());
10688         assertSame(calibrator.getListener(), this);
10689         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10690         assertFalse(calibrator.isReady());
10691         assertFalse(calibrator.isRunning());
10692         assertNull(calibrator.getEstimatedBiases());
10693         assertFalse(calibrator.getEstimatedBiases(null));
10694         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10695         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10696         assertNull(calibrator.getEstimatedBiasFx());
10697         assertNull(calibrator.getEstimatedBiasFy());
10698         assertNull(calibrator.getEstimatedBiasFz());
10699         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10700         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10701         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10702         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10703         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10704         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10705         assertNull(calibrator.getEstimatedMa());
10706         assertNull(calibrator.getEstimatedSx());
10707         assertNull(calibrator.getEstimatedSy());
10708         assertNull(calibrator.getEstimatedSz());
10709         assertNull(calibrator.getEstimatedMxy());
10710         assertNull(calibrator.getEstimatedMxz());
10711         assertNull(calibrator.getEstimatedMyx());
10712         assertNull(calibrator.getEstimatedMyz());
10713         assertNull(calibrator.getEstimatedMzx());
10714         assertNull(calibrator.getEstimatedMzy());
10715         assertNull(calibrator.getEstimatedCovariance());
10716         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10717     }
10718 
10719     @Test
10720     public void testConstructor95() throws WrongSizeException {
10721         final Matrix ba = generateBa();
10722         final double biasX = ba.getElementAtIndex(0);
10723         final double biasY = ba.getElementAtIndex(1);
10724         final double biasZ = ba.getElementAtIndex(2);
10725 
10726         final Acceleration bx = new Acceleration(biasX,
10727                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10728         final Acceleration by = new Acceleration(biasY,
10729                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10730         final Acceleration bz = new Acceleration(biasZ,
10731                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10732 
10733         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10734         final double latitude = Math.toRadians(
10735                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10736         final double longitude = Math.toRadians(
10737                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10738         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10739         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10740         final NEDVelocity nedVelocity = new NEDVelocity();
10741         final ECEFPosition ecefPosition = new ECEFPosition();
10742         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10743         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10744                 ecefPosition, ecefVelocity);
10745 
10746         final KnownPositionAccelerometerCalibrator calibrator =
10747                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10748                         bx, by, bz);
10749 
10750         // check default values
10751         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10752         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10753         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10754         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10755         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10756         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10757         final Acceleration bx2 = new Acceleration(0.0,
10758                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10759         calibrator.getInitialBiasXAsAcceleration(bx2);
10760         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10761         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10762         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10763         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10764         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10765         final Acceleration by2 = new Acceleration(0.0,
10766                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10767         calibrator.getInitialBiasYAsAcceleration(by2);
10768         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10769         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10770         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10771         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10772         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10773         final Acceleration bz2 = new Acceleration(0.0,
10774                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10775         calibrator.getInitialBiasZAsAcceleration(bz2);
10776         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10777         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10778         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10779         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10780         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10781         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10782         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10783         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10784         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10785         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10786         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10787         final double[] bias1 = calibrator.getInitialBias();
10788         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10789         final double[] bias2 = new double[3];
10790         calibrator.getInitialBias(bias2);
10791         assertArrayEquals(bias1, bias2, 0.0);
10792         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10793         assertEquals(b1, ba);
10794         final Matrix b2 = new Matrix(3, 1);
10795         calibrator.getInitialBiasAsMatrix(b2);
10796         assertEquals(b1, b2);
10797         final Matrix ma1 = calibrator.getInitialMa();
10798         assertEquals(ma1, new Matrix(3, 3));
10799         final Matrix ma2 = new Matrix(3, 3);
10800         calibrator.getInitialMa(ma2);
10801         assertEquals(ma1, ma2);
10802         assertSame(calibrator.getEcefPosition(), ecefPosition);
10803         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10804         final NEDPosition nedPosition1 = new NEDPosition();
10805         assertTrue(calibrator.getNedPosition(nedPosition1));
10806         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10807         assertNull(calibrator.getMeasurements());
10808         assertFalse(calibrator.isCommonAxisUsed());
10809         assertNull(calibrator.getListener());
10810         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10811         assertFalse(calibrator.isReady());
10812         assertFalse(calibrator.isRunning());
10813         assertNull(calibrator.getEstimatedBiases());
10814         assertFalse(calibrator.getEstimatedBiases(null));
10815         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10816         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10817         assertNull(calibrator.getEstimatedBiasFx());
10818         assertNull(calibrator.getEstimatedBiasFy());
10819         assertNull(calibrator.getEstimatedBiasFz());
10820         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10821         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10822         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10823         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10824         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10825         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10826         assertNull(calibrator.getEstimatedMa());
10827         assertNull(calibrator.getEstimatedSx());
10828         assertNull(calibrator.getEstimatedSy());
10829         assertNull(calibrator.getEstimatedSz());
10830         assertNull(calibrator.getEstimatedMxy());
10831         assertNull(calibrator.getEstimatedMxz());
10832         assertNull(calibrator.getEstimatedMyx());
10833         assertNull(calibrator.getEstimatedMyz());
10834         assertNull(calibrator.getEstimatedMzx());
10835         assertNull(calibrator.getEstimatedMzy());
10836         assertNull(calibrator.getEstimatedCovariance());
10837         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10838     }
10839 
10840     @Test
10841     public void testConstructor96() throws WrongSizeException {
10842         final Matrix ba = generateBa();
10843         final double biasX = ba.getElementAtIndex(0);
10844         final double biasY = ba.getElementAtIndex(1);
10845         final double biasZ = ba.getElementAtIndex(2);
10846 
10847         final Acceleration bx = new Acceleration(biasX,
10848                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10849         final Acceleration by = new Acceleration(biasY,
10850                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10851         final Acceleration bz = new Acceleration(biasZ,
10852                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10853 
10854         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10855         final double latitude = Math.toRadians(
10856                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10857         final double longitude = Math.toRadians(
10858                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10859         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10860         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10861         final NEDVelocity nedVelocity = new NEDVelocity();
10862         final ECEFPosition ecefPosition = new ECEFPosition();
10863         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10864         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10865                 ecefPosition, ecefVelocity);
10866 
10867         final KnownPositionAccelerometerCalibrator calibrator =
10868                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10869                         bx, by, bz, this);
10870 
10871         // check default values
10872         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10873         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10874         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10875         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10876         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10877         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10878         final Acceleration bx2 = new Acceleration(0.0,
10879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10880         calibrator.getInitialBiasXAsAcceleration(bx2);
10881         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10882         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10883         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10884         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10885         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10886         final Acceleration by2 = new Acceleration(0.0,
10887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10888         calibrator.getInitialBiasYAsAcceleration(by2);
10889         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10890         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10891         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10892         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10893         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10894         final Acceleration bz2 = new Acceleration(0.0,
10895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10896         calibrator.getInitialBiasZAsAcceleration(bz2);
10897         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10898         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10899         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10900         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10901         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10902         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10903         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10904         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10905         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10906         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10907         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10908         final double[] bias1 = calibrator.getInitialBias();
10909         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
10910         final double[] bias2 = new double[3];
10911         calibrator.getInitialBias(bias2);
10912         assertArrayEquals(bias1, bias2, 0.0);
10913         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10914         assertEquals(b1, ba);
10915         final Matrix b2 = new Matrix(3, 1);
10916         calibrator.getInitialBiasAsMatrix(b2);
10917         assertEquals(b1, b2);
10918         final Matrix ma1 = calibrator.getInitialMa();
10919         assertEquals(ma1, new Matrix(3, 3));
10920         final Matrix ma2 = new Matrix(3, 3);
10921         calibrator.getInitialMa(ma2);
10922         assertEquals(ma1, ma2);
10923         assertSame(calibrator.getEcefPosition(), ecefPosition);
10924         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10925         final NEDPosition nedPosition1 = new NEDPosition();
10926         assertTrue(calibrator.getNedPosition(nedPosition1));
10927         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10928         assertNull(calibrator.getMeasurements());
10929         assertFalse(calibrator.isCommonAxisUsed());
10930         assertSame(calibrator.getListener(), this);
10931         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10932         assertFalse(calibrator.isReady());
10933         assertFalse(calibrator.isRunning());
10934         assertNull(calibrator.getEstimatedBiases());
10935         assertFalse(calibrator.getEstimatedBiases(null));
10936         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10937         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10938         assertNull(calibrator.getEstimatedBiasFx());
10939         assertNull(calibrator.getEstimatedBiasFy());
10940         assertNull(calibrator.getEstimatedBiasFz());
10941         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10942         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10943         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10944         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10945         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10946         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10947         assertNull(calibrator.getEstimatedMa());
10948         assertNull(calibrator.getEstimatedSx());
10949         assertNull(calibrator.getEstimatedSy());
10950         assertNull(calibrator.getEstimatedSz());
10951         assertNull(calibrator.getEstimatedMxy());
10952         assertNull(calibrator.getEstimatedMxz());
10953         assertNull(calibrator.getEstimatedMyx());
10954         assertNull(calibrator.getEstimatedMyz());
10955         assertNull(calibrator.getEstimatedMzx());
10956         assertNull(calibrator.getEstimatedMzy());
10957         assertNull(calibrator.getEstimatedCovariance());
10958         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10959     }
10960 
10961     @Test
10962     public void testConstructor97() throws WrongSizeException {
10963         final Collection<StandardDeviationBodyKinematics> measurements =
10964                 Collections.emptyList();
10965 
10966         final Matrix ba = generateBa();
10967         final double biasX = ba.getElementAtIndex(0);
10968         final double biasY = ba.getElementAtIndex(1);
10969         final double biasZ = ba.getElementAtIndex(2);
10970 
10971         final Acceleration bx = new Acceleration(biasX,
10972                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10973         final Acceleration by = new Acceleration(biasY,
10974                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10975         final Acceleration bz = new Acceleration(biasZ,
10976                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
10977 
10978         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10979         final double latitude = Math.toRadians(
10980                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10981         final double longitude = Math.toRadians(
10982                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10983         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10984         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10985         final NEDVelocity nedVelocity = new NEDVelocity();
10986         final ECEFPosition ecefPosition = new ECEFPosition();
10987         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10988         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10989                 ecefPosition, ecefVelocity);
10990 
10991         final KnownPositionAccelerometerCalibrator calibrator =
10992                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10993                         bx, by, bz);
10994 
10995         // check default values
10996         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10997         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10998         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10999         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11000         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11001         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11002         final Acceleration bx2 = new Acceleration(0.0,
11003                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11004         calibrator.getInitialBiasXAsAcceleration(bx2);
11005         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11006         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11007         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11008         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11009         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11010         final Acceleration by2 = new Acceleration(0.0,
11011                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11012         calibrator.getInitialBiasYAsAcceleration(by2);
11013         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11014         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11015         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11016         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11017         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11018         final Acceleration bz2 = new Acceleration(0.0,
11019                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11020         calibrator.getInitialBiasZAsAcceleration(bz2);
11021         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11022         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11023         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11024         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11025         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11026         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11027         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11028         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11029         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11030         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11031         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11032         final double[] bias1 = calibrator.getInitialBias();
11033         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11034         final double[] bias2 = new double[3];
11035         calibrator.getInitialBias(bias2);
11036         assertArrayEquals(bias1, bias2, 0.0);
11037         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11038         assertEquals(b1, ba);
11039         final Matrix b2 = new Matrix(3, 1);
11040         calibrator.getInitialBiasAsMatrix(b2);
11041         assertEquals(b1, b2);
11042         final Matrix ma1 = calibrator.getInitialMa();
11043         assertEquals(ma1, new Matrix(3, 3));
11044         final Matrix ma2 = new Matrix(3, 3);
11045         calibrator.getInitialMa(ma2);
11046         assertEquals(ma1, ma2);
11047         assertSame(calibrator.getEcefPosition(), ecefPosition);
11048         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11049         final NEDPosition nedPosition1 = new NEDPosition();
11050         assertTrue(calibrator.getNedPosition(nedPosition1));
11051         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11052         assertSame(calibrator.getMeasurements(), measurements);
11053         assertFalse(calibrator.isCommonAxisUsed());
11054         assertNull(calibrator.getListener());
11055         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11056         assertFalse(calibrator.isReady());
11057         assertFalse(calibrator.isRunning());
11058         assertNull(calibrator.getEstimatedBiases());
11059         assertFalse(calibrator.getEstimatedBiases(null));
11060         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11061         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11062         assertNull(calibrator.getEstimatedBiasFx());
11063         assertNull(calibrator.getEstimatedBiasFy());
11064         assertNull(calibrator.getEstimatedBiasFz());
11065         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11066         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11067         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11068         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11069         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11070         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11071         assertNull(calibrator.getEstimatedMa());
11072         assertNull(calibrator.getEstimatedSx());
11073         assertNull(calibrator.getEstimatedSy());
11074         assertNull(calibrator.getEstimatedSz());
11075         assertNull(calibrator.getEstimatedMxy());
11076         assertNull(calibrator.getEstimatedMxz());
11077         assertNull(calibrator.getEstimatedMyx());
11078         assertNull(calibrator.getEstimatedMyz());
11079         assertNull(calibrator.getEstimatedMzx());
11080         assertNull(calibrator.getEstimatedMzy());
11081         assertNull(calibrator.getEstimatedCovariance());
11082         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11083     }
11084 
11085     @Test
11086     public void testConstructor98() throws WrongSizeException {
11087         final Collection<StandardDeviationBodyKinematics> measurements =
11088                 Collections.emptyList();
11089 
11090         final Matrix ba = generateBa();
11091         final double biasX = ba.getElementAtIndex(0);
11092         final double biasY = ba.getElementAtIndex(1);
11093         final double biasZ = ba.getElementAtIndex(2);
11094 
11095         final Acceleration bx = new Acceleration(biasX,
11096                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11097         final Acceleration by = new Acceleration(biasY,
11098                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11099         final Acceleration bz = new Acceleration(biasZ,
11100                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11101 
11102         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11103         final double latitude = Math.toRadians(
11104                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11105         final double longitude = Math.toRadians(
11106                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11107         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11108         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11109         final NEDVelocity nedVelocity = new NEDVelocity();
11110         final ECEFPosition ecefPosition = new ECEFPosition();
11111         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11112         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11113                 ecefPosition, ecefVelocity);
11114 
11115         final KnownPositionAccelerometerCalibrator calibrator =
11116                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11117                         bx, by, bz, this);
11118 
11119         // check default values
11120         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11121         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11122         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11123         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11124         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11125         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126         final Acceleration bx2 = new Acceleration(0.0,
11127                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11128         calibrator.getInitialBiasXAsAcceleration(bx2);
11129         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11130         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11131         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11132         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11133         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11134         final Acceleration by2 = new Acceleration(0.0,
11135                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11136         calibrator.getInitialBiasYAsAcceleration(by2);
11137         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11138         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11139         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11140         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11141         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11142         final Acceleration bz2 = new Acceleration(0.0,
11143                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11144         calibrator.getInitialBiasZAsAcceleration(bz2);
11145         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11146         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11147         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11148         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11149         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11150         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11151         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11152         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11153         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11154         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11155         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11156         final double[] bias1 = calibrator.getInitialBias();
11157         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11158         final double[] bias2 = new double[3];
11159         calibrator.getInitialBias(bias2);
11160         assertArrayEquals(bias1, bias2, 0.0);
11161         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11162         assertEquals(b1, ba);
11163         final Matrix b2 = new Matrix(3, 1);
11164         calibrator.getInitialBiasAsMatrix(b2);
11165         assertEquals(b1, b2);
11166         final Matrix ma1 = calibrator.getInitialMa();
11167         assertEquals(ma1, new Matrix(3, 3));
11168         final Matrix ma2 = new Matrix(3, 3);
11169         calibrator.getInitialMa(ma2);
11170         assertEquals(ma1, ma2);
11171         assertSame(calibrator.getEcefPosition(), ecefPosition);
11172         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11173         final NEDPosition nedPosition1 = new NEDPosition();
11174         assertTrue(calibrator.getNedPosition(nedPosition1));
11175         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11176         assertSame(calibrator.getMeasurements(), measurements);
11177         assertFalse(calibrator.isCommonAxisUsed());
11178         assertSame(calibrator.getListener(), this);
11179         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11180         assertFalse(calibrator.isReady());
11181         assertFalse(calibrator.isRunning());
11182         assertNull(calibrator.getEstimatedBiases());
11183         assertFalse(calibrator.getEstimatedBiases(null));
11184         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11185         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11186         assertNull(calibrator.getEstimatedBiasFx());
11187         assertNull(calibrator.getEstimatedBiasFy());
11188         assertNull(calibrator.getEstimatedBiasFz());
11189         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11190         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11191         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11192         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11193         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11194         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11195         assertNull(calibrator.getEstimatedMa());
11196         assertNull(calibrator.getEstimatedSx());
11197         assertNull(calibrator.getEstimatedSy());
11198         assertNull(calibrator.getEstimatedSz());
11199         assertNull(calibrator.getEstimatedMxy());
11200         assertNull(calibrator.getEstimatedMxz());
11201         assertNull(calibrator.getEstimatedMyx());
11202         assertNull(calibrator.getEstimatedMyz());
11203         assertNull(calibrator.getEstimatedMzx());
11204         assertNull(calibrator.getEstimatedMzy());
11205         assertNull(calibrator.getEstimatedCovariance());
11206         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11207     }
11208 
11209     @Test
11210     public void testConstructor99() throws WrongSizeException {
11211         final Matrix ba = generateBa();
11212         final double biasX = ba.getElementAtIndex(0);
11213         final double biasY = ba.getElementAtIndex(1);
11214         final double biasZ = ba.getElementAtIndex(2);
11215 
11216         final Acceleration bx = new Acceleration(biasX,
11217                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11218         final Acceleration by = new Acceleration(biasY,
11219                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11220         final Acceleration bz = new Acceleration(biasZ,
11221                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11222 
11223         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11224         final double latitude = Math.toRadians(
11225                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11226         final double longitude = Math.toRadians(
11227                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11228         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11229         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11230         final NEDVelocity nedVelocity = new NEDVelocity();
11231         final ECEFPosition ecefPosition = new ECEFPosition();
11232         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11233         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11234                 ecefPosition, ecefVelocity);
11235 
11236         final KnownPositionAccelerometerCalibrator calibrator =
11237                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11238                         true, bx, by, bz);
11239 
11240         // check default values
11241         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11242         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11243         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11244         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11245         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11246         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11247         final Acceleration bx2 = new Acceleration(0.0,
11248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11249         calibrator.getInitialBiasXAsAcceleration(bx2);
11250         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11251         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11252         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11253         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11254         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11255         final Acceleration by2 = new Acceleration(0.0,
11256                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11257         calibrator.getInitialBiasYAsAcceleration(by2);
11258         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11259         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11260         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11261         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11262         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11263         final Acceleration bz2 = new Acceleration(0.0,
11264                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11265         calibrator.getInitialBiasZAsAcceleration(bz2);
11266         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11267         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11268         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11269         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11270         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11271         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11272         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11273         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11274         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11275         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11276         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11277         final double[] bias1 = calibrator.getInitialBias();
11278         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11279         final double[] bias2 = new double[3];
11280         calibrator.getInitialBias(bias2);
11281         assertArrayEquals(bias1, bias2, 0.0);
11282         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11283         assertEquals(b1, ba);
11284         final Matrix b2 = new Matrix(3, 1);
11285         calibrator.getInitialBiasAsMatrix(b2);
11286         assertEquals(b1, b2);
11287         final Matrix ma1 = calibrator.getInitialMa();
11288         assertEquals(ma1, new Matrix(3, 3));
11289         final Matrix ma2 = new Matrix(3, 3);
11290         calibrator.getInitialMa(ma2);
11291         assertEquals(ma1, ma2);
11292         assertSame(calibrator.getEcefPosition(), ecefPosition);
11293         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11294         final NEDPosition nedPosition1 = new NEDPosition();
11295         assertTrue(calibrator.getNedPosition(nedPosition1));
11296         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11297         assertNull(calibrator.getMeasurements());
11298         assertTrue(calibrator.isCommonAxisUsed());
11299         assertNull(calibrator.getListener());
11300         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11301         assertFalse(calibrator.isReady());
11302         assertFalse(calibrator.isRunning());
11303         assertNull(calibrator.getEstimatedBiases());
11304         assertFalse(calibrator.getEstimatedBiases(null));
11305         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11306         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11307         assertNull(calibrator.getEstimatedBiasFx());
11308         assertNull(calibrator.getEstimatedBiasFy());
11309         assertNull(calibrator.getEstimatedBiasFz());
11310         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11311         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11312         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11313         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11314         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11315         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11316         assertNull(calibrator.getEstimatedMa());
11317         assertNull(calibrator.getEstimatedSx());
11318         assertNull(calibrator.getEstimatedSy());
11319         assertNull(calibrator.getEstimatedSz());
11320         assertNull(calibrator.getEstimatedMxy());
11321         assertNull(calibrator.getEstimatedMxz());
11322         assertNull(calibrator.getEstimatedMyx());
11323         assertNull(calibrator.getEstimatedMyz());
11324         assertNull(calibrator.getEstimatedMzx());
11325         assertNull(calibrator.getEstimatedMzy());
11326         assertNull(calibrator.getEstimatedCovariance());
11327         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11328     }
11329 
11330     @Test
11331     public void testConstructor100() throws WrongSizeException {
11332         final Matrix ba = generateBa();
11333         final double biasX = ba.getElementAtIndex(0);
11334         final double biasY = ba.getElementAtIndex(1);
11335         final double biasZ = ba.getElementAtIndex(2);
11336 
11337         final Acceleration bx = new Acceleration(biasX,
11338                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11339         final Acceleration by = new Acceleration(biasY,
11340                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11341         final Acceleration bz = new Acceleration(biasZ,
11342                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11343 
11344         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11345         final double latitude = Math.toRadians(
11346                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11347         final double longitude = Math.toRadians(
11348                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11349         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11350         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11351         final NEDVelocity nedVelocity = new NEDVelocity();
11352         final ECEFPosition ecefPosition = new ECEFPosition();
11353         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11354         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11355                 ecefPosition, ecefVelocity);
11356 
11357         final KnownPositionAccelerometerCalibrator calibrator =
11358                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11359                         true, bx, by, bz, this);
11360 
11361         // check default values
11362         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11363         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11364         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11365         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11366         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11367         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11368         final Acceleration bx2 = new Acceleration(0.0,
11369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11370         calibrator.getInitialBiasXAsAcceleration(bx2);
11371         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11372         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11373         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11374         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11375         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11376         final Acceleration by2 = new Acceleration(0.0,
11377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11378         calibrator.getInitialBiasYAsAcceleration(by2);
11379         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11380         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11381         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11382         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11383         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11384         final Acceleration bz2 = new Acceleration(0.0,
11385                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11386         calibrator.getInitialBiasZAsAcceleration(bz2);
11387         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11388         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11389         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11390         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11391         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11392         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11393         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11394         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11395         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11396         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11397         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11398         final double[] bias1 = calibrator.getInitialBias();
11399         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11400         final double[] bias2 = new double[3];
11401         calibrator.getInitialBias(bias2);
11402         assertArrayEquals(bias1, bias2, 0.0);
11403         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11404         assertEquals(b1, ba);
11405         final Matrix b2 = new Matrix(3, 1);
11406         calibrator.getInitialBiasAsMatrix(b2);
11407         assertEquals(b1, b2);
11408         final Matrix ma1 = calibrator.getInitialMa();
11409         assertEquals(ma1, new Matrix(3, 3));
11410         final Matrix ma2 = new Matrix(3, 3);
11411         calibrator.getInitialMa(ma2);
11412         assertEquals(ma1, ma2);
11413         assertSame(calibrator.getEcefPosition(), ecefPosition);
11414         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11415         final NEDPosition nedPosition1 = new NEDPosition();
11416         assertTrue(calibrator.getNedPosition(nedPosition1));
11417         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11418         assertNull(calibrator.getMeasurements());
11419         assertTrue(calibrator.isCommonAxisUsed());
11420         assertSame(calibrator.getListener(), this);
11421         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11422         assertFalse(calibrator.isReady());
11423         assertFalse(calibrator.isRunning());
11424         assertNull(calibrator.getEstimatedBiases());
11425         assertFalse(calibrator.getEstimatedBiases(null));
11426         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11427         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11428         assertNull(calibrator.getEstimatedBiasFx());
11429         assertNull(calibrator.getEstimatedBiasFy());
11430         assertNull(calibrator.getEstimatedBiasFz());
11431         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11432         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11433         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11434         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11435         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11436         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11437         assertNull(calibrator.getEstimatedMa());
11438         assertNull(calibrator.getEstimatedSx());
11439         assertNull(calibrator.getEstimatedSy());
11440         assertNull(calibrator.getEstimatedSz());
11441         assertNull(calibrator.getEstimatedMxy());
11442         assertNull(calibrator.getEstimatedMxz());
11443         assertNull(calibrator.getEstimatedMyx());
11444         assertNull(calibrator.getEstimatedMyz());
11445         assertNull(calibrator.getEstimatedMzx());
11446         assertNull(calibrator.getEstimatedMzy());
11447         assertNull(calibrator.getEstimatedCovariance());
11448         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11449     }
11450 
11451     @Test
11452     public void testConstructor101() throws WrongSizeException {
11453         final Collection<StandardDeviationBodyKinematics> measurements =
11454                 Collections.emptyList();
11455 
11456         final Matrix ba = generateBa();
11457         final double biasX = ba.getElementAtIndex(0);
11458         final double biasY = ba.getElementAtIndex(1);
11459         final double biasZ = ba.getElementAtIndex(2);
11460 
11461         final Acceleration bx = new Acceleration(biasX,
11462                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11463         final Acceleration by = new Acceleration(biasY,
11464                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11465         final Acceleration bz = new Acceleration(biasZ,
11466                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11467 
11468         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11469         final double latitude = Math.toRadians(
11470                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11471         final double longitude = Math.toRadians(
11472                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11473         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11474         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11475         final NEDVelocity nedVelocity = new NEDVelocity();
11476         final ECEFPosition ecefPosition = new ECEFPosition();
11477         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11478         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11479                 ecefPosition, ecefVelocity);
11480 
11481         final KnownPositionAccelerometerCalibrator calibrator =
11482                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11483                         true, bx, by, bz);
11484 
11485         // check default values
11486         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11487         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11488         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11489         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11490         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11491         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11492         final Acceleration bx2 = new Acceleration(0.0,
11493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11494         calibrator.getInitialBiasXAsAcceleration(bx2);
11495         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11496         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11497         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11498         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11499         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11500         final Acceleration by2 = new Acceleration(0.0,
11501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11502         calibrator.getInitialBiasYAsAcceleration(by2);
11503         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11504         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11505         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11506         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11507         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11508         final Acceleration bz2 = new Acceleration(0.0,
11509                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11510         calibrator.getInitialBiasZAsAcceleration(bz2);
11511         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11512         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11513         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11514         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11515         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11516         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11517         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11518         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11519         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11520         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11521         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11522         final double[] bias1 = calibrator.getInitialBias();
11523         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11524         final double[] bias2 = new double[3];
11525         calibrator.getInitialBias(bias2);
11526         assertArrayEquals(bias1, bias2, 0.0);
11527         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11528         assertEquals(b1, ba);
11529         final Matrix b2 = new Matrix(3, 1);
11530         calibrator.getInitialBiasAsMatrix(b2);
11531         assertEquals(b1, b2);
11532         final Matrix ma1 = calibrator.getInitialMa();
11533         assertEquals(ma1, new Matrix(3, 3));
11534         final Matrix ma2 = new Matrix(3, 3);
11535         calibrator.getInitialMa(ma2);
11536         assertEquals(ma1, ma2);
11537         assertSame(calibrator.getEcefPosition(), ecefPosition);
11538         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11539         final NEDPosition nedPosition1 = new NEDPosition();
11540         assertTrue(calibrator.getNedPosition(nedPosition1));
11541         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11542         assertSame(calibrator.getMeasurements(), measurements);
11543         assertTrue(calibrator.isCommonAxisUsed());
11544         assertNull(calibrator.getListener());
11545         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11546         assertFalse(calibrator.isReady());
11547         assertFalse(calibrator.isRunning());
11548         assertNull(calibrator.getEstimatedBiases());
11549         assertFalse(calibrator.getEstimatedBiases(null));
11550         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11551         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11552         assertNull(calibrator.getEstimatedBiasFx());
11553         assertNull(calibrator.getEstimatedBiasFy());
11554         assertNull(calibrator.getEstimatedBiasFz());
11555         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11556         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11557         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11558         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11559         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11560         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11561         assertNull(calibrator.getEstimatedMa());
11562         assertNull(calibrator.getEstimatedSx());
11563         assertNull(calibrator.getEstimatedSy());
11564         assertNull(calibrator.getEstimatedSz());
11565         assertNull(calibrator.getEstimatedMxy());
11566         assertNull(calibrator.getEstimatedMxz());
11567         assertNull(calibrator.getEstimatedMyx());
11568         assertNull(calibrator.getEstimatedMyz());
11569         assertNull(calibrator.getEstimatedMzx());
11570         assertNull(calibrator.getEstimatedMzy());
11571         assertNull(calibrator.getEstimatedCovariance());
11572         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11573     }
11574 
11575     @Test
11576     public void testConstructor102() throws WrongSizeException {
11577         final Collection<StandardDeviationBodyKinematics> measurements =
11578                 Collections.emptyList();
11579 
11580         final Matrix ba = generateBa();
11581         final double biasX = ba.getElementAtIndex(0);
11582         final double biasY = ba.getElementAtIndex(1);
11583         final double biasZ = ba.getElementAtIndex(2);
11584 
11585         final Acceleration bx = new Acceleration(biasX,
11586                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11587         final Acceleration by = new Acceleration(biasY,
11588                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11589         final Acceleration bz = new Acceleration(biasZ,
11590                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11591 
11592         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11593         final double latitude = Math.toRadians(
11594                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11595         final double longitude = Math.toRadians(
11596                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11597         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11598         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11599         final NEDVelocity nedVelocity = new NEDVelocity();
11600         final ECEFPosition ecefPosition = new ECEFPosition();
11601         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11602         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11603                 ecefPosition, ecefVelocity);
11604 
11605         final KnownPositionAccelerometerCalibrator calibrator =
11606                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11607                         true, bx, by, bz, this);
11608 
11609         // check default values
11610         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11611         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11612         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11613         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11614         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11615         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11616         final Acceleration bx2 = new Acceleration(0.0,
11617                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11618         calibrator.getInitialBiasXAsAcceleration(bx2);
11619         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11620         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11621         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11622         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11623         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11624         final Acceleration by2 = new Acceleration(0.0,
11625                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11626         calibrator.getInitialBiasYAsAcceleration(by2);
11627         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11628         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11629         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11630         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11631         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11632         final Acceleration bz2 = new Acceleration(0.0,
11633                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11634         calibrator.getInitialBiasZAsAcceleration(bz2);
11635         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11636         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11637         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11638         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11639         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11640         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11641         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11642         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11643         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11644         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11645         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11646         final double[] bias1 = calibrator.getInitialBias();
11647         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11648         final double[] bias2 = new double[3];
11649         calibrator.getInitialBias(bias2);
11650         assertArrayEquals(bias1, bias2, 0.0);
11651         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11652         assertEquals(b1, ba);
11653         final Matrix b2 = new Matrix(3, 1);
11654         calibrator.getInitialBiasAsMatrix(b2);
11655         assertEquals(b1, b2);
11656         final Matrix ma1 = calibrator.getInitialMa();
11657         assertEquals(ma1, new Matrix(3, 3));
11658         final Matrix ma2 = new Matrix(3, 3);
11659         calibrator.getInitialMa(ma2);
11660         assertEquals(ma1, ma2);
11661         assertSame(calibrator.getEcefPosition(), ecefPosition);
11662         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11663         final NEDPosition nedPosition1 = new NEDPosition();
11664         assertTrue(calibrator.getNedPosition(nedPosition1));
11665         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11666         assertSame(calibrator.getMeasurements(), measurements);
11667         assertTrue(calibrator.isCommonAxisUsed());
11668         assertSame(calibrator.getListener(), this);
11669         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11670         assertFalse(calibrator.isReady());
11671         assertFalse(calibrator.isRunning());
11672         assertNull(calibrator.getEstimatedBiases());
11673         assertFalse(calibrator.getEstimatedBiases(null));
11674         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11675         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11676         assertNull(calibrator.getEstimatedBiasFx());
11677         assertNull(calibrator.getEstimatedBiasFy());
11678         assertNull(calibrator.getEstimatedBiasFz());
11679         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11680         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11681         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11682         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11683         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11684         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11685         assertNull(calibrator.getEstimatedMa());
11686         assertNull(calibrator.getEstimatedSx());
11687         assertNull(calibrator.getEstimatedSy());
11688         assertNull(calibrator.getEstimatedSz());
11689         assertNull(calibrator.getEstimatedMxy());
11690         assertNull(calibrator.getEstimatedMxz());
11691         assertNull(calibrator.getEstimatedMyx());
11692         assertNull(calibrator.getEstimatedMyz());
11693         assertNull(calibrator.getEstimatedMzx());
11694         assertNull(calibrator.getEstimatedMzy());
11695         assertNull(calibrator.getEstimatedCovariance());
11696         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11697     }
11698 
11699     @Test
11700     public void testConstructor103() throws WrongSizeException {
11701         final Matrix ba = generateBa();
11702         final double biasX = ba.getElementAtIndex(0);
11703         final double biasY = ba.getElementAtIndex(1);
11704         final double biasZ = ba.getElementAtIndex(2);
11705 
11706         final Matrix ma = generateMaCommonAxis();
11707         final double sx = ma.getElementAt(0, 0);
11708         final double sy = ma.getElementAt(1, 1);
11709         final double sz = ma.getElementAt(2, 2);
11710 
11711         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11712         final double latitude = Math.toRadians(
11713                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11714         final double longitude = Math.toRadians(
11715                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11716         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11717         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11718         final NEDVelocity nedVelocity = new NEDVelocity();
11719         final ECEFPosition ecefPosition = new ECEFPosition();
11720         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11721         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11722                 ecefPosition, ecefVelocity);
11723 
11724         final KnownPositionAccelerometerCalibrator calibrator =
11725                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11726                         biasX, biasY, biasZ, sx, sy, sz);
11727 
11728         // check default values
11729         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11730         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11731         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11732         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11733         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11734         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11735         final Acceleration bx2 = new Acceleration(0.0,
11736                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11737         calibrator.getInitialBiasXAsAcceleration(bx2);
11738         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11739         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11740         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11741         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11742         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11743         final Acceleration by2 = new Acceleration(0.0,
11744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11745         calibrator.getInitialBiasYAsAcceleration(by2);
11746         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11747         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11748         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11749         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11750         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11751         final Acceleration bz2 = new Acceleration(0.0,
11752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11753         calibrator.getInitialBiasZAsAcceleration(bz2);
11754         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11755         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11756         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11757         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11758         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11759         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11760         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11761         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11762         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11763         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11764         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11765         final double[] bias1 = calibrator.getInitialBias();
11766         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11767         final double[] bias2 = new double[3];
11768         calibrator.getInitialBias(bias2);
11769         assertArrayEquals(bias1, bias2, 0.0);
11770         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11771         assertEquals(b1, ba);
11772         final Matrix b2 = new Matrix(3, 1);
11773         calibrator.getInitialBiasAsMatrix(b2);
11774         assertEquals(b1, b2);
11775         final Matrix ma1 = calibrator.getInitialMa();
11776         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
11777         final Matrix ma2 = new Matrix(3, 3);
11778         calibrator.getInitialMa(ma2);
11779         assertEquals(ma1, ma2);
11780         assertSame(calibrator.getEcefPosition(), ecefPosition);
11781         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11782         final NEDPosition nedPosition1 = new NEDPosition();
11783         assertTrue(calibrator.getNedPosition(nedPosition1));
11784         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11785         assertNull(calibrator.getMeasurements());
11786         assertFalse(calibrator.isCommonAxisUsed());
11787         assertNull(calibrator.getListener());
11788         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11789         assertFalse(calibrator.isReady());
11790         assertFalse(calibrator.isRunning());
11791         assertNull(calibrator.getEstimatedBiases());
11792         assertFalse(calibrator.getEstimatedBiases(null));
11793         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11794         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11795         assertNull(calibrator.getEstimatedBiasFx());
11796         assertNull(calibrator.getEstimatedBiasFy());
11797         assertNull(calibrator.getEstimatedBiasFz());
11798         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11799         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11800         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11801         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11802         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11803         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11804         assertNull(calibrator.getEstimatedMa());
11805         assertNull(calibrator.getEstimatedSx());
11806         assertNull(calibrator.getEstimatedSy());
11807         assertNull(calibrator.getEstimatedSz());
11808         assertNull(calibrator.getEstimatedMxy());
11809         assertNull(calibrator.getEstimatedMxz());
11810         assertNull(calibrator.getEstimatedMyx());
11811         assertNull(calibrator.getEstimatedMyz());
11812         assertNull(calibrator.getEstimatedMzx());
11813         assertNull(calibrator.getEstimatedMzy());
11814         assertNull(calibrator.getEstimatedCovariance());
11815         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11816     }
11817 
11818     @Test
11819     public void testConstructor104() throws WrongSizeException {
11820         final Collection<StandardDeviationBodyKinematics> measurements =
11821                 Collections.emptyList();
11822 
11823         final Matrix ba = generateBa();
11824         final double biasX = ba.getElementAtIndex(0);
11825         final double biasY = ba.getElementAtIndex(1);
11826         final double biasZ = ba.getElementAtIndex(2);
11827 
11828         final Matrix ma = generateMaCommonAxis();
11829         final double sx = ma.getElementAt(0, 0);
11830         final double sy = ma.getElementAt(1, 1);
11831         final double sz = ma.getElementAt(2, 2);
11832 
11833         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11834         final double latitude = Math.toRadians(
11835                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11836         final double longitude = Math.toRadians(
11837                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11838         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11839         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11840         final NEDVelocity nedVelocity = new NEDVelocity();
11841         final ECEFPosition ecefPosition = new ECEFPosition();
11842         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11843         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11844                 ecefPosition, ecefVelocity);
11845 
11846         final KnownPositionAccelerometerCalibrator calibrator =
11847                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11848                         biasX, biasY, biasZ, sx, sy, sz);
11849 
11850         // check default values
11851         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11852         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11853         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11854         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11855         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11856         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11857         final Acceleration bx2 = new Acceleration(0.0,
11858                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11859         calibrator.getInitialBiasXAsAcceleration(bx2);
11860         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11861         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11862         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11863         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11864         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11865         final Acceleration by2 = new Acceleration(0.0,
11866                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11867         calibrator.getInitialBiasYAsAcceleration(by2);
11868         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11869         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11870         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11871         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11872         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11873         final Acceleration bz2 = new Acceleration(0.0,
11874                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11875         calibrator.getInitialBiasZAsAcceleration(bz2);
11876         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11877         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11878         assertEquals(calibrator.getInitialSx(), sx, 0.0);
11879         assertEquals(calibrator.getInitialSy(), sy, 0.0);
11880         assertEquals(calibrator.getInitialSz(), sz, 0.0);
11881         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11882         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11883         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11884         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11885         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11886         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11887         final double[] bias1 = calibrator.getInitialBias();
11888         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
11889         final double[] bias2 = new double[3];
11890         calibrator.getInitialBias(bias2);
11891         assertArrayEquals(bias1, bias2, 0.0);
11892         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11893         assertEquals(b1, ba);
11894         final Matrix b2 = new Matrix(3, 1);
11895         calibrator.getInitialBiasAsMatrix(b2);
11896         assertEquals(b1, b2);
11897         final Matrix ma1 = calibrator.getInitialMa();
11898         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
11899         final Matrix ma2 = new Matrix(3, 3);
11900         calibrator.getInitialMa(ma2);
11901         assertEquals(ma1, ma2);
11902         assertSame(calibrator.getEcefPosition(), ecefPosition);
11903         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11904         final NEDPosition nedPosition1 = new NEDPosition();
11905         assertTrue(calibrator.getNedPosition(nedPosition1));
11906         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11907         assertSame(calibrator.getMeasurements(), measurements);
11908         assertFalse(calibrator.isCommonAxisUsed());
11909         assertNull(calibrator.getListener());
11910         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11911         assertFalse(calibrator.isReady());
11912         assertFalse(calibrator.isRunning());
11913         assertNull(calibrator.getEstimatedBiases());
11914         assertFalse(calibrator.getEstimatedBiases(null));
11915         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11916         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11917         assertNull(calibrator.getEstimatedBiasFx());
11918         assertNull(calibrator.getEstimatedBiasFy());
11919         assertNull(calibrator.getEstimatedBiasFz());
11920         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11921         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11922         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11923         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11924         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11925         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11926         assertNull(calibrator.getEstimatedMa());
11927         assertNull(calibrator.getEstimatedSx());
11928         assertNull(calibrator.getEstimatedSy());
11929         assertNull(calibrator.getEstimatedSz());
11930         assertNull(calibrator.getEstimatedMxy());
11931         assertNull(calibrator.getEstimatedMxz());
11932         assertNull(calibrator.getEstimatedMyx());
11933         assertNull(calibrator.getEstimatedMyz());
11934         assertNull(calibrator.getEstimatedMzx());
11935         assertNull(calibrator.getEstimatedMzy());
11936         assertNull(calibrator.getEstimatedCovariance());
11937         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11938     }
11939 
11940     @Test
11941     public void testConstructor105() throws WrongSizeException {
11942         final Collection<StandardDeviationBodyKinematics> measurements =
11943                 Collections.emptyList();
11944 
11945         final Matrix ba = generateBa();
11946         final double biasX = ba.getElementAtIndex(0);
11947         final double biasY = ba.getElementAtIndex(1);
11948         final double biasZ = ba.getElementAtIndex(2);
11949 
11950         final Matrix ma = generateMaCommonAxis();
11951         final double sx = ma.getElementAt(0, 0);
11952         final double sy = ma.getElementAt(1, 1);
11953         final double sz = ma.getElementAt(2, 2);
11954 
11955         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11956         final double latitude = Math.toRadians(
11957                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11958         final double longitude = Math.toRadians(
11959                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11960         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11961         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11962         final NEDVelocity nedVelocity = new NEDVelocity();
11963         final ECEFPosition ecefPosition = new ECEFPosition();
11964         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11965         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11966                 ecefPosition, ecefVelocity);
11967 
11968         final KnownPositionAccelerometerCalibrator calibrator =
11969                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11970                         biasX, biasY, biasZ, sx, sy, sz, this);
11971 
11972         // check default values
11973         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11974         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11975         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11976         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11977         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11978         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11979         final Acceleration bx2 = new Acceleration(0.0,
11980                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11981         calibrator.getInitialBiasXAsAcceleration(bx2);
11982         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11983         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11984         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11985         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11986         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11987         final Acceleration by2 = new Acceleration(0.0,
11988                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11989         calibrator.getInitialBiasYAsAcceleration(by2);
11990         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11991         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11992         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11993         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11994         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11995         final Acceleration bz2 = new Acceleration(0.0,
11996                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11997         calibrator.getInitialBiasZAsAcceleration(bz2);
11998         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11999         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12000         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12001         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12002         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12003         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12004         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12005         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12006         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12007         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12008         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12009         final double[] bias1 = calibrator.getInitialBias();
12010         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12011         final double[] bias2 = new double[3];
12012         calibrator.getInitialBias(bias2);
12013         assertArrayEquals(bias1, bias2, 0.0);
12014         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12015         assertEquals(b1, ba);
12016         final Matrix b2 = new Matrix(3, 1);
12017         calibrator.getInitialBiasAsMatrix(b2);
12018         assertEquals(b1, b2);
12019         final Matrix ma1 = calibrator.getInitialMa();
12020         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12021         final Matrix ma2 = new Matrix(3, 3);
12022         calibrator.getInitialMa(ma2);
12023         assertEquals(ma1, ma2);
12024         assertSame(calibrator.getEcefPosition(), ecefPosition);
12025         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12026         final NEDPosition nedPosition1 = new NEDPosition();
12027         assertTrue(calibrator.getNedPosition(nedPosition1));
12028         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12029         assertSame(calibrator.getMeasurements(), measurements);
12030         assertFalse(calibrator.isCommonAxisUsed());
12031         assertSame(calibrator.getListener(), this);
12032         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12033         assertFalse(calibrator.isReady());
12034         assertFalse(calibrator.isRunning());
12035         assertNull(calibrator.getEstimatedBiases());
12036         assertFalse(calibrator.getEstimatedBiases(null));
12037         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12038         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12039         assertNull(calibrator.getEstimatedBiasFx());
12040         assertNull(calibrator.getEstimatedBiasFy());
12041         assertNull(calibrator.getEstimatedBiasFz());
12042         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12043         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12044         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12045         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12046         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12047         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12048         assertNull(calibrator.getEstimatedMa());
12049         assertNull(calibrator.getEstimatedSx());
12050         assertNull(calibrator.getEstimatedSy());
12051         assertNull(calibrator.getEstimatedSz());
12052         assertNull(calibrator.getEstimatedMxy());
12053         assertNull(calibrator.getEstimatedMxz());
12054         assertNull(calibrator.getEstimatedMyx());
12055         assertNull(calibrator.getEstimatedMyz());
12056         assertNull(calibrator.getEstimatedMzx());
12057         assertNull(calibrator.getEstimatedMzy());
12058         assertNull(calibrator.getEstimatedCovariance());
12059         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12060     }
12061 
12062     @Test
12063     public void testConstructor106() throws WrongSizeException {
12064         final Matrix ba = generateBa();
12065         final double biasX = ba.getElementAtIndex(0);
12066         final double biasY = ba.getElementAtIndex(1);
12067         final double biasZ = ba.getElementAtIndex(2);
12068 
12069         final Matrix ma = generateMaCommonAxis();
12070         final double sx = ma.getElementAt(0, 0);
12071         final double sy = ma.getElementAt(1, 1);
12072         final double sz = ma.getElementAt(2, 2);
12073 
12074         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12075         final double latitude = Math.toRadians(
12076                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12077         final double longitude = Math.toRadians(
12078                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12079         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12080         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12081         final NEDVelocity nedVelocity = new NEDVelocity();
12082         final ECEFPosition ecefPosition = new ECEFPosition();
12083         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12084         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12085                 ecefPosition, ecefVelocity);
12086 
12087         final KnownPositionAccelerometerCalibrator calibrator =
12088                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12089                         true, biasX, biasY, biasZ, sx, sy, sz);
12090 
12091         // check default values
12092         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12093         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12094         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12095         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12096         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12097         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12098         final Acceleration bx2 = new Acceleration(0.0,
12099                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12100         calibrator.getInitialBiasXAsAcceleration(bx2);
12101         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12102         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12103         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12104         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12105         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12106         final Acceleration by2 = new Acceleration(0.0,
12107                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12108         calibrator.getInitialBiasYAsAcceleration(by2);
12109         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12110         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12111         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12112         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12113         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12114         final Acceleration bz2 = new Acceleration(0.0,
12115                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12116         calibrator.getInitialBiasZAsAcceleration(bz2);
12117         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12118         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12119         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12120         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12121         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12122         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12123         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12124         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12125         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12126         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12127         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12128         final double[] bias1 = calibrator.getInitialBias();
12129         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12130         final double[] bias2 = new double[3];
12131         calibrator.getInitialBias(bias2);
12132         assertArrayEquals(bias1, bias2, 0.0);
12133         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12134         assertEquals(b1, ba);
12135         final Matrix b2 = new Matrix(3, 1);
12136         calibrator.getInitialBiasAsMatrix(b2);
12137         assertEquals(b1, b2);
12138         final Matrix ma1 = calibrator.getInitialMa();
12139         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12140         final Matrix ma2 = new Matrix(3, 3);
12141         calibrator.getInitialMa(ma2);
12142         assertEquals(ma1, ma2);
12143         assertSame(calibrator.getEcefPosition(), ecefPosition);
12144         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12145         final NEDPosition nedPosition1 = new NEDPosition();
12146         assertTrue(calibrator.getNedPosition(nedPosition1));
12147         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12148         assertNull(calibrator.getMeasurements());
12149         assertTrue(calibrator.isCommonAxisUsed());
12150         assertNull(calibrator.getListener());
12151         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12152         assertFalse(calibrator.isReady());
12153         assertFalse(calibrator.isRunning());
12154         assertNull(calibrator.getEstimatedBiases());
12155         assertFalse(calibrator.getEstimatedBiases(null));
12156         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12157         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12158         assertNull(calibrator.getEstimatedBiasFx());
12159         assertNull(calibrator.getEstimatedBiasFy());
12160         assertNull(calibrator.getEstimatedBiasFz());
12161         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12162         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12163         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12164         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12165         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12166         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12167         assertNull(calibrator.getEstimatedMa());
12168         assertNull(calibrator.getEstimatedSx());
12169         assertNull(calibrator.getEstimatedSy());
12170         assertNull(calibrator.getEstimatedSz());
12171         assertNull(calibrator.getEstimatedMxy());
12172         assertNull(calibrator.getEstimatedMxz());
12173         assertNull(calibrator.getEstimatedMyx());
12174         assertNull(calibrator.getEstimatedMyz());
12175         assertNull(calibrator.getEstimatedMzx());
12176         assertNull(calibrator.getEstimatedMzy());
12177         assertNull(calibrator.getEstimatedCovariance());
12178         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12179     }
12180 
12181     @Test
12182     public void testConstructor107() throws WrongSizeException {
12183         final Matrix ba = generateBa();
12184         final double biasX = ba.getElementAtIndex(0);
12185         final double biasY = ba.getElementAtIndex(1);
12186         final double biasZ = ba.getElementAtIndex(2);
12187 
12188         final Matrix ma = generateMaCommonAxis();
12189         final double sx = ma.getElementAt(0, 0);
12190         final double sy = ma.getElementAt(1, 1);
12191         final double sz = ma.getElementAt(2, 2);
12192 
12193         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12194         final double latitude = Math.toRadians(
12195                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12196         final double longitude = Math.toRadians(
12197                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12198         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12199         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12200         final NEDVelocity nedVelocity = new NEDVelocity();
12201         final ECEFPosition ecefPosition = new ECEFPosition();
12202         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12203         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12204                 ecefPosition, ecefVelocity);
12205 
12206         final KnownPositionAccelerometerCalibrator calibrator =
12207                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12208                         true, biasX, biasY, biasZ, sx, sy, sz,
12209                         this);
12210 
12211         // check default values
12212         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12213         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12214         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12215         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12216         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12217         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12218         final Acceleration bx2 = new Acceleration(0.0,
12219                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12220         calibrator.getInitialBiasXAsAcceleration(bx2);
12221         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12222         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12223         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12224         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12225         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12226         final Acceleration by2 = new Acceleration(0.0,
12227                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12228         calibrator.getInitialBiasYAsAcceleration(by2);
12229         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12230         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12231         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12232         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12233         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12234         final Acceleration bz2 = new Acceleration(0.0,
12235                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12236         calibrator.getInitialBiasZAsAcceleration(bz2);
12237         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12238         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12239         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12240         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12241         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12242         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12243         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12244         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12245         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12246         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12247         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12248         final double[] bias1 = calibrator.getInitialBias();
12249         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12250         final double[] bias2 = new double[3];
12251         calibrator.getInitialBias(bias2);
12252         assertArrayEquals(bias1, bias2, 0.0);
12253         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12254         assertEquals(b1, ba);
12255         final Matrix b2 = new Matrix(3, 1);
12256         calibrator.getInitialBiasAsMatrix(b2);
12257         assertEquals(b1, b2);
12258         final Matrix ma1 = calibrator.getInitialMa();
12259         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12260         final Matrix ma2 = new Matrix(3, 3);
12261         calibrator.getInitialMa(ma2);
12262         assertEquals(ma1, ma2);
12263         assertSame(calibrator.getEcefPosition(), ecefPosition);
12264         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12265         final NEDPosition nedPosition1 = new NEDPosition();
12266         assertTrue(calibrator.getNedPosition(nedPosition1));
12267         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12268         assertNull(calibrator.getMeasurements());
12269         assertTrue(calibrator.isCommonAxisUsed());
12270         assertSame(calibrator.getListener(), this);
12271         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12272         assertFalse(calibrator.isReady());
12273         assertFalse(calibrator.isRunning());
12274         assertNull(calibrator.getEstimatedBiases());
12275         assertFalse(calibrator.getEstimatedBiases(null));
12276         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12277         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12278         assertNull(calibrator.getEstimatedBiasFx());
12279         assertNull(calibrator.getEstimatedBiasFy());
12280         assertNull(calibrator.getEstimatedBiasFz());
12281         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12282         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12283         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12284         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12285         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12286         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12287         assertNull(calibrator.getEstimatedMa());
12288         assertNull(calibrator.getEstimatedSx());
12289         assertNull(calibrator.getEstimatedSy());
12290         assertNull(calibrator.getEstimatedSz());
12291         assertNull(calibrator.getEstimatedMxy());
12292         assertNull(calibrator.getEstimatedMxz());
12293         assertNull(calibrator.getEstimatedMyx());
12294         assertNull(calibrator.getEstimatedMyz());
12295         assertNull(calibrator.getEstimatedMzx());
12296         assertNull(calibrator.getEstimatedMzy());
12297         assertNull(calibrator.getEstimatedCovariance());
12298         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12299     }
12300 
12301     @Test
12302     public void testConstructor108() throws WrongSizeException {
12303         final Collection<StandardDeviationBodyKinematics> measurements =
12304                 Collections.emptyList();
12305 
12306         final Matrix ba = generateBa();
12307         final double biasX = ba.getElementAtIndex(0);
12308         final double biasY = ba.getElementAtIndex(1);
12309         final double biasZ = ba.getElementAtIndex(2);
12310 
12311         final Matrix ma = generateMaCommonAxis();
12312         final double sx = ma.getElementAt(0, 0);
12313         final double sy = ma.getElementAt(1, 1);
12314         final double sz = ma.getElementAt(2, 2);
12315 
12316         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12317         final double latitude = Math.toRadians(
12318                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12319         final double longitude = Math.toRadians(
12320                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12321         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12322         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12323         final NEDVelocity nedVelocity = new NEDVelocity();
12324         final ECEFPosition ecefPosition = new ECEFPosition();
12325         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12326         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12327                 ecefPosition, ecefVelocity);
12328 
12329         final KnownPositionAccelerometerCalibrator calibrator =
12330                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12331                         true, biasX, biasY, biasZ, sx, sy, sz);
12332 
12333         // check default values
12334         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12335         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12336         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12337         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12338         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12339         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12340         final Acceleration bx2 = new Acceleration(0.0,
12341                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12342         calibrator.getInitialBiasXAsAcceleration(bx2);
12343         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12344         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12345         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12346         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12347         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12348         final Acceleration by2 = new Acceleration(0.0,
12349                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12350         calibrator.getInitialBiasYAsAcceleration(by2);
12351         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12352         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12353         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12354         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12355         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12356         final Acceleration bz2 = new Acceleration(0.0,
12357                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12358         calibrator.getInitialBiasZAsAcceleration(bz2);
12359         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12360         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12361         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12362         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12363         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12364         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12365         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12366         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12367         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12368         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12369         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12370         final double[] bias1 = calibrator.getInitialBias();
12371         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12372         final double[] bias2 = new double[3];
12373         calibrator.getInitialBias(bias2);
12374         assertArrayEquals(bias1, bias2, 0.0);
12375         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12376         assertEquals(b1, ba);
12377         final Matrix b2 = new Matrix(3, 1);
12378         calibrator.getInitialBiasAsMatrix(b2);
12379         assertEquals(b1, b2);
12380         final Matrix ma1 = calibrator.getInitialMa();
12381         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12382         final Matrix ma2 = new Matrix(3, 3);
12383         calibrator.getInitialMa(ma2);
12384         assertEquals(ma1, ma2);
12385         assertSame(calibrator.getEcefPosition(), ecefPosition);
12386         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12387         final NEDPosition nedPosition1 = new NEDPosition();
12388         assertTrue(calibrator.getNedPosition(nedPosition1));
12389         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12390         assertSame(calibrator.getMeasurements(), measurements);
12391         assertTrue(calibrator.isCommonAxisUsed());
12392         assertNull(calibrator.getListener());
12393         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12394         assertFalse(calibrator.isReady());
12395         assertFalse(calibrator.isRunning());
12396         assertNull(calibrator.getEstimatedBiases());
12397         assertFalse(calibrator.getEstimatedBiases(null));
12398         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12399         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12400         assertNull(calibrator.getEstimatedBiasFx());
12401         assertNull(calibrator.getEstimatedBiasFy());
12402         assertNull(calibrator.getEstimatedBiasFz());
12403         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12404         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12405         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12406         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12407         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12408         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12409         assertNull(calibrator.getEstimatedMa());
12410         assertNull(calibrator.getEstimatedSx());
12411         assertNull(calibrator.getEstimatedSy());
12412         assertNull(calibrator.getEstimatedSz());
12413         assertNull(calibrator.getEstimatedMxy());
12414         assertNull(calibrator.getEstimatedMxz());
12415         assertNull(calibrator.getEstimatedMyx());
12416         assertNull(calibrator.getEstimatedMyz());
12417         assertNull(calibrator.getEstimatedMzx());
12418         assertNull(calibrator.getEstimatedMzy());
12419         assertNull(calibrator.getEstimatedCovariance());
12420         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12421     }
12422 
12423     @Test
12424     public void testConstructor109() throws WrongSizeException {
12425         final Collection<StandardDeviationBodyKinematics> measurements =
12426                 Collections.emptyList();
12427 
12428         final Matrix ba = generateBa();
12429         final double biasX = ba.getElementAtIndex(0);
12430         final double biasY = ba.getElementAtIndex(1);
12431         final double biasZ = ba.getElementAtIndex(2);
12432 
12433         final Matrix ma = generateMaCommonAxis();
12434         final double sx = ma.getElementAt(0, 0);
12435         final double sy = ma.getElementAt(1, 1);
12436         final double sz = ma.getElementAt(2, 2);
12437 
12438         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12439         final double latitude = Math.toRadians(
12440                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12441         final double longitude = Math.toRadians(
12442                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12443         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12444         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12445         final NEDVelocity nedVelocity = new NEDVelocity();
12446         final ECEFPosition ecefPosition = new ECEFPosition();
12447         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12448         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12449                 ecefPosition, ecefVelocity);
12450 
12451         final KnownPositionAccelerometerCalibrator calibrator =
12452                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12453                         true, biasX, biasY, biasZ, sx, sy, sz,
12454                         this);
12455 
12456         // check default values
12457         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12458         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12459         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12460         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12461         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12462         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12463         final Acceleration bx2 = new Acceleration(0.0,
12464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12465         calibrator.getInitialBiasXAsAcceleration(bx2);
12466         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12467         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12468         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12469         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12470         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12471         final Acceleration by2 = new Acceleration(0.0,
12472                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12473         calibrator.getInitialBiasYAsAcceleration(by2);
12474         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12475         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12476         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12477         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12478         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12479         final Acceleration bz2 = new Acceleration(0.0,
12480                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12481         calibrator.getInitialBiasZAsAcceleration(bz2);
12482         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12483         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12484         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12485         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12486         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12487         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12488         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12489         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12490         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12491         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12492         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12493         final double[] bias1 = calibrator.getInitialBias();
12494         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12495         final double[] bias2 = new double[3];
12496         calibrator.getInitialBias(bias2);
12497         assertArrayEquals(bias1, bias2, 0.0);
12498         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12499         assertEquals(b1, ba);
12500         final Matrix b2 = new Matrix(3, 1);
12501         calibrator.getInitialBiasAsMatrix(b2);
12502         assertEquals(b1, b2);
12503         final Matrix ma1 = calibrator.getInitialMa();
12504         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12505         final Matrix ma2 = new Matrix(3, 3);
12506         calibrator.getInitialMa(ma2);
12507         assertEquals(ma1, ma2);
12508         assertSame(calibrator.getEcefPosition(), ecefPosition);
12509         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12510         final NEDPosition nedPosition1 = new NEDPosition();
12511         assertTrue(calibrator.getNedPosition(nedPosition1));
12512         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12513         assertSame(calibrator.getMeasurements(), measurements);
12514         assertTrue(calibrator.isCommonAxisUsed());
12515         assertSame(calibrator.getListener(), this);
12516         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12517         assertFalse(calibrator.isReady());
12518         assertFalse(calibrator.isRunning());
12519         assertNull(calibrator.getEstimatedBiases());
12520         assertFalse(calibrator.getEstimatedBiases(null));
12521         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12522         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12523         assertNull(calibrator.getEstimatedBiasFx());
12524         assertNull(calibrator.getEstimatedBiasFy());
12525         assertNull(calibrator.getEstimatedBiasFz());
12526         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12527         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12528         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12529         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12530         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12531         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12532         assertNull(calibrator.getEstimatedMa());
12533         assertNull(calibrator.getEstimatedSx());
12534         assertNull(calibrator.getEstimatedSy());
12535         assertNull(calibrator.getEstimatedSz());
12536         assertNull(calibrator.getEstimatedMxy());
12537         assertNull(calibrator.getEstimatedMxz());
12538         assertNull(calibrator.getEstimatedMyx());
12539         assertNull(calibrator.getEstimatedMyz());
12540         assertNull(calibrator.getEstimatedMzx());
12541         assertNull(calibrator.getEstimatedMzy());
12542         assertNull(calibrator.getEstimatedCovariance());
12543         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12544     }
12545 
12546     @Test
12547     public void testConstructor110() throws WrongSizeException {
12548         final Matrix ba = generateBa();
12549         final double biasX = ba.getElementAtIndex(0);
12550         final double biasY = ba.getElementAtIndex(1);
12551         final double biasZ = ba.getElementAtIndex(2);
12552 
12553         final Acceleration bx = new Acceleration(biasX,
12554                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12555         final Acceleration by = new Acceleration(biasY,
12556                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12557         final Acceleration bz = new Acceleration(biasZ,
12558                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12559 
12560         final Matrix ma = generateMaCommonAxis();
12561         final double sx = ma.getElementAt(0, 0);
12562         final double sy = ma.getElementAt(1, 1);
12563         final double sz = ma.getElementAt(2, 2);
12564 
12565         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12566         final double latitude = Math.toRadians(
12567                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12568         final double longitude = Math.toRadians(
12569                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12570         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12571         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12572         final NEDVelocity nedVelocity = new NEDVelocity();
12573         final ECEFPosition ecefPosition = new ECEFPosition();
12574         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12575         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12576                 ecefPosition, ecefVelocity);
12577 
12578         final KnownPositionAccelerometerCalibrator calibrator =
12579                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12580                         bx, by, bz, sx, sy, sz);
12581 
12582         // check default values
12583         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12584         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12585         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12586         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12587         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12588         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12589         final Acceleration bx2 = new Acceleration(0.0,
12590                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12591         calibrator.getInitialBiasXAsAcceleration(bx2);
12592         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12593         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12594         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12595         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12596         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12597         final Acceleration by2 = new Acceleration(0.0,
12598                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12599         calibrator.getInitialBiasYAsAcceleration(by2);
12600         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12601         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12602         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12603         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12604         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12605         final Acceleration bz2 = new Acceleration(0.0,
12606                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12607         calibrator.getInitialBiasZAsAcceleration(bz2);
12608         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12609         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12610         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12611         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12612         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12613         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12614         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12615         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12616         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12617         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12618         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12619         final double[] bias1 = calibrator.getInitialBias();
12620         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12621         final double[] bias2 = new double[3];
12622         calibrator.getInitialBias(bias2);
12623         assertArrayEquals(bias1, bias2, 0.0);
12624         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12625         assertEquals(b1, ba);
12626         final Matrix b2 = new Matrix(3, 1);
12627         calibrator.getInitialBiasAsMatrix(b2);
12628         assertEquals(b1, b2);
12629         final Matrix ma1 = calibrator.getInitialMa();
12630         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12631         final Matrix ma2 = new Matrix(3, 3);
12632         calibrator.getInitialMa(ma2);
12633         assertEquals(ma1, ma2);
12634         assertSame(calibrator.getEcefPosition(), ecefPosition);
12635         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12636         final NEDPosition nedPosition1 = new NEDPosition();
12637         assertTrue(calibrator.getNedPosition(nedPosition1));
12638         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12639         assertNull(calibrator.getMeasurements());
12640         assertFalse(calibrator.isCommonAxisUsed());
12641         assertNull(calibrator.getListener());
12642         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12643         assertFalse(calibrator.isReady());
12644         assertFalse(calibrator.isRunning());
12645         assertNull(calibrator.getEstimatedBiases());
12646         assertFalse(calibrator.getEstimatedBiases(null));
12647         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12648         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12649         assertNull(calibrator.getEstimatedBiasFx());
12650         assertNull(calibrator.getEstimatedBiasFy());
12651         assertNull(calibrator.getEstimatedBiasFz());
12652         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12653         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12654         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12655         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12656         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12657         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12658         assertNull(calibrator.getEstimatedMa());
12659         assertNull(calibrator.getEstimatedSx());
12660         assertNull(calibrator.getEstimatedSy());
12661         assertNull(calibrator.getEstimatedSz());
12662         assertNull(calibrator.getEstimatedMxy());
12663         assertNull(calibrator.getEstimatedMxz());
12664         assertNull(calibrator.getEstimatedMyx());
12665         assertNull(calibrator.getEstimatedMyz());
12666         assertNull(calibrator.getEstimatedMzx());
12667         assertNull(calibrator.getEstimatedMzy());
12668         assertNull(calibrator.getEstimatedCovariance());
12669         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12670     }
12671 
12672     @Test
12673     public void testConstructor111() throws WrongSizeException {
12674         final Matrix ba = generateBa();
12675         final double biasX = ba.getElementAtIndex(0);
12676         final double biasY = ba.getElementAtIndex(1);
12677         final double biasZ = ba.getElementAtIndex(2);
12678 
12679         final Acceleration bx = new Acceleration(biasX,
12680                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12681         final Acceleration by = new Acceleration(biasY,
12682                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12683         final Acceleration bz = new Acceleration(biasZ,
12684                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12685 
12686         final Matrix ma = generateMaCommonAxis();
12687         final double sx = ma.getElementAt(0, 0);
12688         final double sy = ma.getElementAt(1, 1);
12689         final double sz = ma.getElementAt(2, 2);
12690 
12691         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12692         final double latitude = Math.toRadians(
12693                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12694         final double longitude = Math.toRadians(
12695                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12696         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12697         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12698         final NEDVelocity nedVelocity = new NEDVelocity();
12699         final ECEFPosition ecefPosition = new ECEFPosition();
12700         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12701         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12702                 ecefPosition, ecefVelocity);
12703 
12704         final KnownPositionAccelerometerCalibrator calibrator =
12705                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12706                         bx, by, bz, sx, sy, sz, this);
12707 
12708         // check default values
12709         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12710         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12711         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12712         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12713         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12714         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12715         final Acceleration bx2 = new Acceleration(0.0,
12716                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12717         calibrator.getInitialBiasXAsAcceleration(bx2);
12718         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12719         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12720         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12721         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12722         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12723         final Acceleration by2 = new Acceleration(0.0,
12724                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12725         calibrator.getInitialBiasYAsAcceleration(by2);
12726         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12727         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12728         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12729         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12730         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12731         final Acceleration bz2 = new Acceleration(0.0,
12732                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12733         calibrator.getInitialBiasZAsAcceleration(bz2);
12734         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12735         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12736         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12737         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12738         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12739         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12740         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12741         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12742         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12743         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12744         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12745         final double[] bias1 = calibrator.getInitialBias();
12746         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12747         final double[] bias2 = new double[3];
12748         calibrator.getInitialBias(bias2);
12749         assertArrayEquals(bias1, bias2, 0.0);
12750         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12751         assertEquals(b1, ba);
12752         final Matrix b2 = new Matrix(3, 1);
12753         calibrator.getInitialBiasAsMatrix(b2);
12754         assertEquals(b1, b2);
12755         final Matrix ma1 = calibrator.getInitialMa();
12756         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12757         final Matrix ma2 = new Matrix(3, 3);
12758         calibrator.getInitialMa(ma2);
12759         assertEquals(ma1, ma2);
12760         assertSame(calibrator.getEcefPosition(), ecefPosition);
12761         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12762         final NEDPosition nedPosition1 = new NEDPosition();
12763         assertTrue(calibrator.getNedPosition(nedPosition1));
12764         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12765         assertNull(calibrator.getMeasurements());
12766         assertFalse(calibrator.isCommonAxisUsed());
12767         assertSame(calibrator.getListener(), this);
12768         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12769         assertFalse(calibrator.isReady());
12770         assertFalse(calibrator.isRunning());
12771         assertNull(calibrator.getEstimatedBiases());
12772         assertFalse(calibrator.getEstimatedBiases(null));
12773         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12774         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12775         assertNull(calibrator.getEstimatedBiasFx());
12776         assertNull(calibrator.getEstimatedBiasFy());
12777         assertNull(calibrator.getEstimatedBiasFz());
12778         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12779         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12780         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12781         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12782         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12783         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12784         assertNull(calibrator.getEstimatedMa());
12785         assertNull(calibrator.getEstimatedSx());
12786         assertNull(calibrator.getEstimatedSy());
12787         assertNull(calibrator.getEstimatedSz());
12788         assertNull(calibrator.getEstimatedMxy());
12789         assertNull(calibrator.getEstimatedMxz());
12790         assertNull(calibrator.getEstimatedMyx());
12791         assertNull(calibrator.getEstimatedMyz());
12792         assertNull(calibrator.getEstimatedMzx());
12793         assertNull(calibrator.getEstimatedMzy());
12794         assertNull(calibrator.getEstimatedCovariance());
12795         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12796     }
12797 
12798     @Test
12799     public void testConstructor112() throws WrongSizeException {
12800         final Collection<StandardDeviationBodyKinematics> measurements =
12801                 Collections.emptyList();
12802 
12803         final Matrix ba = generateBa();
12804         final double biasX = ba.getElementAtIndex(0);
12805         final double biasY = ba.getElementAtIndex(1);
12806         final double biasZ = ba.getElementAtIndex(2);
12807 
12808         final Acceleration bx = new Acceleration(biasX,
12809                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12810         final Acceleration by = new Acceleration(biasY,
12811                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12812         final Acceleration bz = new Acceleration(biasZ,
12813                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12814 
12815         final Matrix ma = generateMaCommonAxis();
12816         final double sx = ma.getElementAt(0, 0);
12817         final double sy = ma.getElementAt(1, 1);
12818         final double sz = ma.getElementAt(2, 2);
12819 
12820         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12821         final double latitude = Math.toRadians(
12822                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12823         final double longitude = Math.toRadians(
12824                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12825         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12826         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12827         final NEDVelocity nedVelocity = new NEDVelocity();
12828         final ECEFPosition ecefPosition = new ECEFPosition();
12829         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12830         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12831                 ecefPosition, ecefVelocity);
12832 
12833         final KnownPositionAccelerometerCalibrator calibrator =
12834                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12835                         bx, by, bz, sx, sy, sz);
12836 
12837         // check default values
12838         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12839         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12840         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12841         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12842         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12843         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12844         final Acceleration bx2 = new Acceleration(0.0,
12845                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12846         calibrator.getInitialBiasXAsAcceleration(bx2);
12847         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12848         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12849         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12850         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12851         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12852         final Acceleration by2 = new Acceleration(0.0,
12853                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12854         calibrator.getInitialBiasYAsAcceleration(by2);
12855         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12856         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12857         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12858         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12859         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12860         final Acceleration bz2 = new Acceleration(0.0,
12861                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12862         calibrator.getInitialBiasZAsAcceleration(bz2);
12863         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12864         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12865         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12866         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12867         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12868         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12869         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12870         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12871         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12872         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12873         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12874         final double[] bias1 = calibrator.getInitialBias();
12875         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
12876         final double[] bias2 = new double[3];
12877         calibrator.getInitialBias(bias2);
12878         assertArrayEquals(bias1, bias2, 0.0);
12879         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12880         assertEquals(b1, ba);
12881         final Matrix b2 = new Matrix(3, 1);
12882         calibrator.getInitialBiasAsMatrix(b2);
12883         assertEquals(b1, b2);
12884         final Matrix ma1 = calibrator.getInitialMa();
12885         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
12886         final Matrix ma2 = new Matrix(3, 3);
12887         calibrator.getInitialMa(ma2);
12888         assertEquals(ma1, ma2);
12889         assertSame(calibrator.getEcefPosition(), ecefPosition);
12890         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12891         final NEDPosition nedPosition1 = new NEDPosition();
12892         assertTrue(calibrator.getNedPosition(nedPosition1));
12893         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12894         assertSame(calibrator.getMeasurements(), measurements);
12895         assertFalse(calibrator.isCommonAxisUsed());
12896         assertNull(calibrator.getListener());
12897         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12898         assertFalse(calibrator.isReady());
12899         assertFalse(calibrator.isRunning());
12900         assertNull(calibrator.getEstimatedBiases());
12901         assertFalse(calibrator.getEstimatedBiases(null));
12902         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12903         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12904         assertNull(calibrator.getEstimatedBiasFx());
12905         assertNull(calibrator.getEstimatedBiasFy());
12906         assertNull(calibrator.getEstimatedBiasFz());
12907         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12908         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12909         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12910         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12911         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12912         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12913         assertNull(calibrator.getEstimatedMa());
12914         assertNull(calibrator.getEstimatedSx());
12915         assertNull(calibrator.getEstimatedSy());
12916         assertNull(calibrator.getEstimatedSz());
12917         assertNull(calibrator.getEstimatedMxy());
12918         assertNull(calibrator.getEstimatedMxz());
12919         assertNull(calibrator.getEstimatedMyx());
12920         assertNull(calibrator.getEstimatedMyz());
12921         assertNull(calibrator.getEstimatedMzx());
12922         assertNull(calibrator.getEstimatedMzy());
12923         assertNull(calibrator.getEstimatedCovariance());
12924         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12925     }
12926 
12927     @Test
12928     public void testConstructor113() throws WrongSizeException {
12929         final Collection<StandardDeviationBodyKinematics> measurements =
12930                 Collections.emptyList();
12931 
12932         final Matrix ba = generateBa();
12933         final double biasX = ba.getElementAtIndex(0);
12934         final double biasY = ba.getElementAtIndex(1);
12935         final double biasZ = ba.getElementAtIndex(2);
12936 
12937         final Acceleration bx = new Acceleration(biasX,
12938                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12939         final Acceleration by = new Acceleration(biasY,
12940                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12941         final Acceleration bz = new Acceleration(biasZ,
12942                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12943 
12944         final Matrix ma = generateMaCommonAxis();
12945         final double sx = ma.getElementAt(0, 0);
12946         final double sy = ma.getElementAt(1, 1);
12947         final double sz = ma.getElementAt(2, 2);
12948 
12949         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12950         final double latitude = Math.toRadians(
12951                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12952         final double longitude = Math.toRadians(
12953                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12954         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12955         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12956         final NEDVelocity nedVelocity = new NEDVelocity();
12957         final ECEFPosition ecefPosition = new ECEFPosition();
12958         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12959         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12960                 ecefPosition, ecefVelocity);
12961 
12962         final KnownPositionAccelerometerCalibrator calibrator =
12963                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12964                         bx, by, bz, sx, sy, sz, this);
12965 
12966         // check default values
12967         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12968         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12969         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12970         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12971         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12972         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12973         final Acceleration bx2 = new Acceleration(0.0,
12974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12975         calibrator.getInitialBiasXAsAcceleration(bx2);
12976         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12977         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12978         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12979         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12980         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12981         final Acceleration by2 = new Acceleration(0.0,
12982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12983         calibrator.getInitialBiasYAsAcceleration(by2);
12984         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12985         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12986         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12987         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12988         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12989         final Acceleration bz2 = new Acceleration(0.0,
12990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12991         calibrator.getInitialBiasZAsAcceleration(bz2);
12992         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12993         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12994         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12995         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12996         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12997         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12998         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12999         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13000         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13001         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13002         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13003         final double[] bias1 = calibrator.getInitialBias();
13004         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13005         final double[] bias2 = new double[3];
13006         calibrator.getInitialBias(bias2);
13007         assertArrayEquals(bias1, bias2, 0.0);
13008         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13009         assertEquals(b1, ba);
13010         final Matrix b2 = new Matrix(3, 1);
13011         calibrator.getInitialBiasAsMatrix(b2);
13012         assertEquals(b1, b2);
13013         final Matrix ma1 = calibrator.getInitialMa();
13014         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13015         final Matrix ma2 = new Matrix(3, 3);
13016         calibrator.getInitialMa(ma2);
13017         assertEquals(ma1, ma2);
13018         assertSame(calibrator.getEcefPosition(), ecefPosition);
13019         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13020         final NEDPosition nedPosition1 = new NEDPosition();
13021         assertTrue(calibrator.getNedPosition(nedPosition1));
13022         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13023         assertSame(calibrator.getMeasurements(), measurements);
13024         assertFalse(calibrator.isCommonAxisUsed());
13025         assertSame(calibrator.getListener(), this);
13026         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13027         assertFalse(calibrator.isReady());
13028         assertFalse(calibrator.isRunning());
13029         assertNull(calibrator.getEstimatedBiases());
13030         assertFalse(calibrator.getEstimatedBiases(null));
13031         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13032         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13033         assertNull(calibrator.getEstimatedBiasFx());
13034         assertNull(calibrator.getEstimatedBiasFy());
13035         assertNull(calibrator.getEstimatedBiasFz());
13036         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13037         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13038         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13039         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13040         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13041         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13042         assertNull(calibrator.getEstimatedMa());
13043         assertNull(calibrator.getEstimatedSx());
13044         assertNull(calibrator.getEstimatedSy());
13045         assertNull(calibrator.getEstimatedSz());
13046         assertNull(calibrator.getEstimatedMxy());
13047         assertNull(calibrator.getEstimatedMxz());
13048         assertNull(calibrator.getEstimatedMyx());
13049         assertNull(calibrator.getEstimatedMyz());
13050         assertNull(calibrator.getEstimatedMzx());
13051         assertNull(calibrator.getEstimatedMzy());
13052         assertNull(calibrator.getEstimatedCovariance());
13053         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13054     }
13055 
13056     @Test
13057     public void testConstructor114() throws WrongSizeException {
13058         final Matrix ba = generateBa();
13059         final double biasX = ba.getElementAtIndex(0);
13060         final double biasY = ba.getElementAtIndex(1);
13061         final double biasZ = ba.getElementAtIndex(2);
13062 
13063         final Acceleration bx = new Acceleration(biasX,
13064                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13065         final Acceleration by = new Acceleration(biasY,
13066                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13067         final Acceleration bz = new Acceleration(biasZ,
13068                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13069 
13070         final Matrix ma = generateMaCommonAxis();
13071         final double sx = ma.getElementAt(0, 0);
13072         final double sy = ma.getElementAt(1, 1);
13073         final double sz = ma.getElementAt(2, 2);
13074 
13075         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13076         final double latitude = Math.toRadians(
13077                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13078         final double longitude = Math.toRadians(
13079                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13080         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13081         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13082         final NEDVelocity nedVelocity = new NEDVelocity();
13083         final ECEFPosition ecefPosition = new ECEFPosition();
13084         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13085         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13086                 ecefPosition, ecefVelocity);
13087 
13088         final KnownPositionAccelerometerCalibrator calibrator =
13089                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13090                         true, bx, by, bz, sx, sy, sz);
13091 
13092         // check default values
13093         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13094         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13095         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13096         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13097         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13098         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13099         final Acceleration bx2 = new Acceleration(0.0,
13100                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13101         calibrator.getInitialBiasXAsAcceleration(bx2);
13102         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13103         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13104         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13105         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13106         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13107         final Acceleration by2 = new Acceleration(0.0,
13108                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13109         calibrator.getInitialBiasYAsAcceleration(by2);
13110         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13111         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13112         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13113         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13114         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13115         final Acceleration bz2 = new Acceleration(0.0,
13116                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13117         calibrator.getInitialBiasZAsAcceleration(bz2);
13118         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13119         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13120         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13121         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13122         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13123         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13124         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13125         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13126         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13127         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13128         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13129         final double[] bias1 = calibrator.getInitialBias();
13130         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13131         final double[] bias2 = new double[3];
13132         calibrator.getInitialBias(bias2);
13133         assertArrayEquals(bias1, bias2, 0.0);
13134         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13135         assertEquals(b1, ba);
13136         final Matrix b2 = new Matrix(3, 1);
13137         calibrator.getInitialBiasAsMatrix(b2);
13138         assertEquals(b1, b2);
13139         final Matrix ma1 = calibrator.getInitialMa();
13140         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13141         final Matrix ma2 = new Matrix(3, 3);
13142         calibrator.getInitialMa(ma2);
13143         assertEquals(ma1, ma2);
13144         assertSame(calibrator.getEcefPosition(), ecefPosition);
13145         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13146         final NEDPosition nedPosition1 = new NEDPosition();
13147         assertTrue(calibrator.getNedPosition(nedPosition1));
13148         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13149         assertNull(calibrator.getMeasurements());
13150         assertTrue(calibrator.isCommonAxisUsed());
13151         assertNull(calibrator.getListener());
13152         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13153         assertFalse(calibrator.isReady());
13154         assertFalse(calibrator.isRunning());
13155         assertNull(calibrator.getEstimatedBiases());
13156         assertFalse(calibrator.getEstimatedBiases(null));
13157         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13158         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13159         assertNull(calibrator.getEstimatedBiasFx());
13160         assertNull(calibrator.getEstimatedBiasFy());
13161         assertNull(calibrator.getEstimatedBiasFz());
13162         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13163         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13164         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13165         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13166         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13167         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13168         assertNull(calibrator.getEstimatedMa());
13169         assertNull(calibrator.getEstimatedSx());
13170         assertNull(calibrator.getEstimatedSy());
13171         assertNull(calibrator.getEstimatedSz());
13172         assertNull(calibrator.getEstimatedMxy());
13173         assertNull(calibrator.getEstimatedMxz());
13174         assertNull(calibrator.getEstimatedMyx());
13175         assertNull(calibrator.getEstimatedMyz());
13176         assertNull(calibrator.getEstimatedMzx());
13177         assertNull(calibrator.getEstimatedMzy());
13178         assertNull(calibrator.getEstimatedCovariance());
13179         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13180     }
13181 
13182     @Test
13183     public void testConstructor115() throws WrongSizeException {
13184         final Matrix ba = generateBa();
13185         final double biasX = ba.getElementAtIndex(0);
13186         final double biasY = ba.getElementAtIndex(1);
13187         final double biasZ = ba.getElementAtIndex(2);
13188 
13189         final Acceleration bx = new Acceleration(biasX,
13190                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13191         final Acceleration by = new Acceleration(biasY,
13192                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13193         final Acceleration bz = new Acceleration(biasZ,
13194                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13195 
13196         final Matrix ma = generateMaCommonAxis();
13197         final double sx = ma.getElementAt(0, 0);
13198         final double sy = ma.getElementAt(1, 1);
13199         final double sz = ma.getElementAt(2, 2);
13200 
13201         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13202         final double latitude = Math.toRadians(
13203                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13204         final double longitude = Math.toRadians(
13205                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13206         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13207         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13208         final NEDVelocity nedVelocity = new NEDVelocity();
13209         final ECEFPosition ecefPosition = new ECEFPosition();
13210         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13211         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13212                 ecefPosition, ecefVelocity);
13213 
13214         final KnownPositionAccelerometerCalibrator calibrator =
13215                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13216                         true, bx, by, bz, sx, sy, sz, this);
13217 
13218         // check default values
13219         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13220         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13221         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13222         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13223         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13224         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13225         final Acceleration bx2 = new Acceleration(0.0,
13226                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13227         calibrator.getInitialBiasXAsAcceleration(bx2);
13228         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13229         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13230         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13231         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13232         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13233         final Acceleration by2 = new Acceleration(0.0,
13234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13235         calibrator.getInitialBiasYAsAcceleration(by2);
13236         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13237         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13238         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13239         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13240         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13241         final Acceleration bz2 = new Acceleration(0.0,
13242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13243         calibrator.getInitialBiasZAsAcceleration(bz2);
13244         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13245         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13246         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13247         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13248         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13249         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13250         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13251         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13252         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13253         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13254         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13255         final double[] bias1 = calibrator.getInitialBias();
13256         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13257         final double[] bias2 = new double[3];
13258         calibrator.getInitialBias(bias2);
13259         assertArrayEquals(bias1, bias2, 0.0);
13260         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13261         assertEquals(b1, ba);
13262         final Matrix b2 = new Matrix(3, 1);
13263         calibrator.getInitialBiasAsMatrix(b2);
13264         assertEquals(b1, b2);
13265         final Matrix ma1 = calibrator.getInitialMa();
13266         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13267         final Matrix ma2 = new Matrix(3, 3);
13268         calibrator.getInitialMa(ma2);
13269         assertEquals(ma1, ma2);
13270         assertSame(calibrator.getEcefPosition(), ecefPosition);
13271         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13272         final NEDPosition nedPosition1 = new NEDPosition();
13273         assertTrue(calibrator.getNedPosition(nedPosition1));
13274         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13275         assertNull(calibrator.getMeasurements());
13276         assertTrue(calibrator.isCommonAxisUsed());
13277         assertSame(calibrator.getListener(), this);
13278         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13279         assertFalse(calibrator.isReady());
13280         assertFalse(calibrator.isRunning());
13281         assertNull(calibrator.getEstimatedBiases());
13282         assertFalse(calibrator.getEstimatedBiases(null));
13283         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13284         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13285         assertNull(calibrator.getEstimatedBiasFx());
13286         assertNull(calibrator.getEstimatedBiasFy());
13287         assertNull(calibrator.getEstimatedBiasFz());
13288         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13289         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13290         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13291         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13292         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13293         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13294         assertNull(calibrator.getEstimatedMa());
13295         assertNull(calibrator.getEstimatedSx());
13296         assertNull(calibrator.getEstimatedSy());
13297         assertNull(calibrator.getEstimatedSz());
13298         assertNull(calibrator.getEstimatedMxy());
13299         assertNull(calibrator.getEstimatedMxz());
13300         assertNull(calibrator.getEstimatedMyx());
13301         assertNull(calibrator.getEstimatedMyz());
13302         assertNull(calibrator.getEstimatedMzx());
13303         assertNull(calibrator.getEstimatedMzy());
13304         assertNull(calibrator.getEstimatedCovariance());
13305         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13306     }
13307 
13308     @Test
13309     public void testConstructor116() throws WrongSizeException {
13310         final Collection<StandardDeviationBodyKinematics> measurements =
13311                 Collections.emptyList();
13312 
13313         final Matrix ba = generateBa();
13314         final double biasX = ba.getElementAtIndex(0);
13315         final double biasY = ba.getElementAtIndex(1);
13316         final double biasZ = ba.getElementAtIndex(2);
13317 
13318         final Acceleration bx = new Acceleration(biasX,
13319                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13320         final Acceleration by = new Acceleration(biasY,
13321                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13322         final Acceleration bz = new Acceleration(biasZ,
13323                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13324 
13325         final Matrix ma = generateMaCommonAxis();
13326         final double sx = ma.getElementAt(0, 0);
13327         final double sy = ma.getElementAt(1, 1);
13328         final double sz = ma.getElementAt(2, 2);
13329 
13330         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13331         final double latitude = Math.toRadians(
13332                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13333         final double longitude = Math.toRadians(
13334                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13335         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13336         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13337         final NEDVelocity nedVelocity = new NEDVelocity();
13338         final ECEFPosition ecefPosition = new ECEFPosition();
13339         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13340         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13341                 ecefPosition, ecefVelocity);
13342 
13343         final KnownPositionAccelerometerCalibrator calibrator =
13344                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13345                         true, bx, by, bz, sx, sy, sz);
13346 
13347         // check default values
13348         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13349         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13350         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13351         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13352         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13353         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13354         final Acceleration bx2 = new Acceleration(0.0,
13355                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13356         calibrator.getInitialBiasXAsAcceleration(bx2);
13357         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13358         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13359         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13360         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13361         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13362         final Acceleration by2 = new Acceleration(0.0,
13363                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13364         calibrator.getInitialBiasYAsAcceleration(by2);
13365         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13366         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13367         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13368         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13369         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13370         final Acceleration bz2 = new Acceleration(0.0,
13371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13372         calibrator.getInitialBiasZAsAcceleration(bz2);
13373         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13374         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13375         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13376         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13377         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13378         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13379         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13380         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13381         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13382         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13383         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13384         final double[] bias1 = calibrator.getInitialBias();
13385         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13386         final double[] bias2 = new double[3];
13387         calibrator.getInitialBias(bias2);
13388         assertArrayEquals(bias1, bias2, 0.0);
13389         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13390         assertEquals(b1, ba);
13391         final Matrix b2 = new Matrix(3, 1);
13392         calibrator.getInitialBiasAsMatrix(b2);
13393         assertEquals(b1, b2);
13394         final Matrix ma1 = calibrator.getInitialMa();
13395         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13396         final Matrix ma2 = new Matrix(3, 3);
13397         calibrator.getInitialMa(ma2);
13398         assertEquals(ma1, ma2);
13399         assertSame(calibrator.getEcefPosition(), ecefPosition);
13400         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13401         final NEDPosition nedPosition1 = new NEDPosition();
13402         assertTrue(calibrator.getNedPosition(nedPosition1));
13403         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13404         assertSame(calibrator.getMeasurements(), measurements);
13405         assertTrue(calibrator.isCommonAxisUsed());
13406         assertNull(calibrator.getListener());
13407         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13408         assertFalse(calibrator.isReady());
13409         assertFalse(calibrator.isRunning());
13410         assertNull(calibrator.getEstimatedBiases());
13411         assertFalse(calibrator.getEstimatedBiases(null));
13412         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13413         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13414         assertNull(calibrator.getEstimatedBiasFx());
13415         assertNull(calibrator.getEstimatedBiasFy());
13416         assertNull(calibrator.getEstimatedBiasFz());
13417         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13418         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13419         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13420         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13421         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13422         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13423         assertNull(calibrator.getEstimatedMa());
13424         assertNull(calibrator.getEstimatedSx());
13425         assertNull(calibrator.getEstimatedSy());
13426         assertNull(calibrator.getEstimatedSz());
13427         assertNull(calibrator.getEstimatedMxy());
13428         assertNull(calibrator.getEstimatedMxz());
13429         assertNull(calibrator.getEstimatedMyx());
13430         assertNull(calibrator.getEstimatedMyz());
13431         assertNull(calibrator.getEstimatedMzx());
13432         assertNull(calibrator.getEstimatedMzy());
13433         assertNull(calibrator.getEstimatedCovariance());
13434         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13435     }
13436 
13437     @Test
13438     public void testConstructor117() throws WrongSizeException {
13439         final Collection<StandardDeviationBodyKinematics> measurements =
13440                 Collections.emptyList();
13441 
13442         final Matrix ba = generateBa();
13443         final double biasX = ba.getElementAtIndex(0);
13444         final double biasY = ba.getElementAtIndex(1);
13445         final double biasZ = ba.getElementAtIndex(2);
13446 
13447         final Acceleration bx = new Acceleration(biasX,
13448                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13449         final Acceleration by = new Acceleration(biasY,
13450                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13451         final Acceleration bz = new Acceleration(biasZ,
13452                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13453 
13454         final Matrix ma = generateMaCommonAxis();
13455         final double sx = ma.getElementAt(0, 0);
13456         final double sy = ma.getElementAt(1, 1);
13457         final double sz = ma.getElementAt(2, 2);
13458 
13459         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13460         final double latitude = Math.toRadians(
13461                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13462         final double longitude = Math.toRadians(
13463                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13464         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13465         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13466         final NEDVelocity nedVelocity = new NEDVelocity();
13467         final ECEFPosition ecefPosition = new ECEFPosition();
13468         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13469         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13470                 ecefPosition, ecefVelocity);
13471 
13472         final KnownPositionAccelerometerCalibrator calibrator =
13473                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13474                         true, bx, by, bz, sx, sy, sz, this);
13475 
13476         // check default values
13477         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13478         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13479         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13480         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13481         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13482         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13483         final Acceleration bx2 = new Acceleration(0.0,
13484                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13485         calibrator.getInitialBiasXAsAcceleration(bx2);
13486         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13487         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13488         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13489         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13490         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13491         final Acceleration by2 = new Acceleration(0.0,
13492                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13493         calibrator.getInitialBiasYAsAcceleration(by2);
13494         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13495         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13496         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13497         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13498         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13499         final Acceleration bz2 = new Acceleration(0.0,
13500                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13501         calibrator.getInitialBiasZAsAcceleration(bz2);
13502         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13503         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13504         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13505         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13506         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13507         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13508         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13509         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13510         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13511         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13512         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13513         final double[] bias1 = calibrator.getInitialBias();
13514         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13515         final double[] bias2 = new double[3];
13516         calibrator.getInitialBias(bias2);
13517         assertArrayEquals(bias1, bias2, 0.0);
13518         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13519         assertEquals(b1, ba);
13520         final Matrix b2 = new Matrix(3, 1);
13521         calibrator.getInitialBiasAsMatrix(b2);
13522         assertEquals(b1, b2);
13523         final Matrix ma1 = calibrator.getInitialMa();
13524         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
13525         final Matrix ma2 = new Matrix(3, 3);
13526         calibrator.getInitialMa(ma2);
13527         assertEquals(ma1, ma2);
13528         assertSame(calibrator.getEcefPosition(), ecefPosition);
13529         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13530         final NEDPosition nedPosition1 = new NEDPosition();
13531         assertTrue(calibrator.getNedPosition(nedPosition1));
13532         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13533         assertSame(calibrator.getMeasurements(), measurements);
13534         assertTrue(calibrator.isCommonAxisUsed());
13535         assertSame(calibrator.getListener(), this);
13536         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13537         assertFalse(calibrator.isReady());
13538         assertFalse(calibrator.isRunning());
13539         assertNull(calibrator.getEstimatedBiases());
13540         assertFalse(calibrator.getEstimatedBiases(null));
13541         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13542         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13543         assertNull(calibrator.getEstimatedBiasFx());
13544         assertNull(calibrator.getEstimatedBiasFy());
13545         assertNull(calibrator.getEstimatedBiasFz());
13546         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13547         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13548         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13549         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13550         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13551         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13552         assertNull(calibrator.getEstimatedMa());
13553         assertNull(calibrator.getEstimatedSx());
13554         assertNull(calibrator.getEstimatedSy());
13555         assertNull(calibrator.getEstimatedSz());
13556         assertNull(calibrator.getEstimatedMxy());
13557         assertNull(calibrator.getEstimatedMxz());
13558         assertNull(calibrator.getEstimatedMyx());
13559         assertNull(calibrator.getEstimatedMyz());
13560         assertNull(calibrator.getEstimatedMzx());
13561         assertNull(calibrator.getEstimatedMzy());
13562         assertNull(calibrator.getEstimatedCovariance());
13563         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13564     }
13565 
13566     @Test
13567     public void testConstructor118() throws WrongSizeException {
13568         final Matrix ba = generateBa();
13569         final double biasX = ba.getElementAtIndex(0);
13570         final double biasY = ba.getElementAtIndex(1);
13571         final double biasZ = ba.getElementAtIndex(2);
13572 
13573         final Matrix ma = generateMaCommonAxis();
13574         final double sx = ma.getElementAt(0, 0);
13575         final double sy = ma.getElementAt(1, 1);
13576         final double sz = ma.getElementAt(2, 2);
13577         final double mxy = ma.getElementAt(0, 1);
13578         final double mxz = ma.getElementAt(0, 2);
13579         final double myx = ma.getElementAt(1, 0);
13580         final double myz = ma.getElementAt(1, 2);
13581         final double mzx = ma.getElementAt(2, 0);
13582         final double mzy = ma.getElementAt(2, 1);
13583 
13584         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13585         final double latitude = Math.toRadians(
13586                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13587         final double longitude = Math.toRadians(
13588                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13589         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13590         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13591         final NEDVelocity nedVelocity = new NEDVelocity();
13592         final ECEFPosition ecefPosition = new ECEFPosition();
13593         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13594         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13595                 ecefPosition, ecefVelocity);
13596 
13597         final KnownPositionAccelerometerCalibrator calibrator =
13598                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13599                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13600                         myx, myz, mzx, mzy);
13601 
13602         // check default values
13603         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13604         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13605         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13606         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13607         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13608         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13609         final Acceleration bx2 = new Acceleration(0.0,
13610                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13611         calibrator.getInitialBiasXAsAcceleration(bx2);
13612         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13613         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13614         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13615         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13616         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13617         final Acceleration by2 = new Acceleration(0.0,
13618                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13619         calibrator.getInitialBiasYAsAcceleration(by2);
13620         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13621         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13622         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13623         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13624         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13625         final Acceleration bz2 = new Acceleration(0.0,
13626                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13627         calibrator.getInitialBiasZAsAcceleration(bz2);
13628         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13629         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13630         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13631         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13632         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13633         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13634         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13635         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13636         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13637         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13638         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13639         final double[] bias1 = calibrator.getInitialBias();
13640         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13641         final double[] bias2 = new double[3];
13642         calibrator.getInitialBias(bias2);
13643         assertArrayEquals(bias1, bias2, 0.0);
13644         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13645         assertEquals(b1, ba);
13646         final Matrix b2 = new Matrix(3, 1);
13647         calibrator.getInitialBiasAsMatrix(b2);
13648         assertEquals(b1, b2);
13649         final Matrix ma1 = new Matrix(3, 3);
13650         ma1.setSubmatrix(0, 0,
13651                 2, 2,
13652                 new double[]{ sx, myx, mzx,
13653                         mxy, sy, mzy,
13654                         mxz, myz, sz});
13655         assertEquals(calibrator.getInitialMa(), ma1);
13656         final Matrix ma2 = new Matrix(3, 3);
13657         calibrator.getInitialMa(ma2);
13658         assertEquals(ma1, ma2);
13659         assertSame(calibrator.getEcefPosition(), ecefPosition);
13660         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13661         final NEDPosition nedPosition1 = new NEDPosition();
13662         assertTrue(calibrator.getNedPosition(nedPosition1));
13663         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13664         assertNull(calibrator.getMeasurements());
13665         assertFalse(calibrator.isCommonAxisUsed());
13666         assertNull(calibrator.getListener());
13667         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13668         assertFalse(calibrator.isReady());
13669         assertFalse(calibrator.isRunning());
13670         assertNull(calibrator.getEstimatedBiases());
13671         assertFalse(calibrator.getEstimatedBiases(null));
13672         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13673         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13674         assertNull(calibrator.getEstimatedBiasFx());
13675         assertNull(calibrator.getEstimatedBiasFy());
13676         assertNull(calibrator.getEstimatedBiasFz());
13677         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13678         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13679         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13680         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13681         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13682         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13683         assertNull(calibrator.getEstimatedMa());
13684         assertNull(calibrator.getEstimatedSx());
13685         assertNull(calibrator.getEstimatedSy());
13686         assertNull(calibrator.getEstimatedSz());
13687         assertNull(calibrator.getEstimatedMxy());
13688         assertNull(calibrator.getEstimatedMxz());
13689         assertNull(calibrator.getEstimatedMyx());
13690         assertNull(calibrator.getEstimatedMyz());
13691         assertNull(calibrator.getEstimatedMzx());
13692         assertNull(calibrator.getEstimatedMzy());
13693         assertNull(calibrator.getEstimatedCovariance());
13694         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13695     }
13696 
13697     @Test
13698     public void testConstructor119() throws WrongSizeException {
13699         final Collection<StandardDeviationBodyKinematics> measurements =
13700                 Collections.emptyList();
13701 
13702         final Matrix ba = generateBa();
13703         final double biasX = ba.getElementAtIndex(0);
13704         final double biasY = ba.getElementAtIndex(1);
13705         final double biasZ = ba.getElementAtIndex(2);
13706 
13707         final Matrix ma = generateMaCommonAxis();
13708         final double sx = ma.getElementAt(0, 0);
13709         final double sy = ma.getElementAt(1, 1);
13710         final double sz = ma.getElementAt(2, 2);
13711         final double mxy = ma.getElementAt(0, 1);
13712         final double mxz = ma.getElementAt(0, 2);
13713         final double myx = ma.getElementAt(1, 0);
13714         final double myz = ma.getElementAt(1, 2);
13715         final double mzx = ma.getElementAt(2, 0);
13716         final double mzy = ma.getElementAt(2, 1);
13717 
13718         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13719         final double latitude = Math.toRadians(
13720                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13721         final double longitude = Math.toRadians(
13722                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13723         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13724         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13725         final NEDVelocity nedVelocity = new NEDVelocity();
13726         final ECEFPosition ecefPosition = new ECEFPosition();
13727         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13728         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13729                 ecefPosition, ecefVelocity);
13730 
13731         final KnownPositionAccelerometerCalibrator calibrator =
13732                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13733                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13734                         myx, myz, mzx, mzy);
13735 
13736         // check default values
13737         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13738         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13739         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13740         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13741         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13742         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13743         final Acceleration bx2 = new Acceleration(0.0,
13744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13745         calibrator.getInitialBiasXAsAcceleration(bx2);
13746         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13747         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13748         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13749         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13750         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13751         final Acceleration by2 = new Acceleration(0.0,
13752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13753         calibrator.getInitialBiasYAsAcceleration(by2);
13754         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13755         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13756         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13757         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13758         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13759         final Acceleration bz2 = new Acceleration(0.0,
13760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13761         calibrator.getInitialBiasZAsAcceleration(bz2);
13762         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13763         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13764         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13765         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13766         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13767         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13768         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13769         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13770         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13771         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13772         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13773         final double[] bias1 = calibrator.getInitialBias();
13774         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13775         final double[] bias2 = new double[3];
13776         calibrator.getInitialBias(bias2);
13777         assertArrayEquals(bias1, bias2, 0.0);
13778         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13779         assertEquals(b1, ba);
13780         final Matrix b2 = new Matrix(3, 1);
13781         calibrator.getInitialBiasAsMatrix(b2);
13782         assertEquals(b1, b2);
13783         final Matrix ma1 = new Matrix(3, 3);
13784         ma1.setSubmatrix(0, 0,
13785                 2, 2,
13786                 new double[]{ sx, myx, mzx,
13787                         mxy, sy, mzy,
13788                         mxz, myz, sz});
13789         assertEquals(calibrator.getInitialMa(), ma1);
13790         final Matrix ma2 = new Matrix(3, 3);
13791         calibrator.getInitialMa(ma2);
13792         assertEquals(ma1, ma2);
13793         assertSame(calibrator.getEcefPosition(), ecefPosition);
13794         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13795         final NEDPosition nedPosition1 = new NEDPosition();
13796         assertTrue(calibrator.getNedPosition(nedPosition1));
13797         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13798         assertSame(calibrator.getMeasurements(), measurements);
13799         assertFalse(calibrator.isCommonAxisUsed());
13800         assertNull(calibrator.getListener());
13801         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13802         assertFalse(calibrator.isReady());
13803         assertFalse(calibrator.isRunning());
13804         assertNull(calibrator.getEstimatedBiases());
13805         assertFalse(calibrator.getEstimatedBiases(null));
13806         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13807         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13808         assertNull(calibrator.getEstimatedBiasFx());
13809         assertNull(calibrator.getEstimatedBiasFy());
13810         assertNull(calibrator.getEstimatedBiasFz());
13811         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13812         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13813         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13814         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13815         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13816         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13817         assertNull(calibrator.getEstimatedMa());
13818         assertNull(calibrator.getEstimatedSx());
13819         assertNull(calibrator.getEstimatedSy());
13820         assertNull(calibrator.getEstimatedSz());
13821         assertNull(calibrator.getEstimatedMxy());
13822         assertNull(calibrator.getEstimatedMxz());
13823         assertNull(calibrator.getEstimatedMyx());
13824         assertNull(calibrator.getEstimatedMyz());
13825         assertNull(calibrator.getEstimatedMzx());
13826         assertNull(calibrator.getEstimatedMzy());
13827         assertNull(calibrator.getEstimatedCovariance());
13828         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13829     }
13830 
13831     @Test
13832     public void testConstructor120() throws WrongSizeException {
13833         final Collection<StandardDeviationBodyKinematics> measurements =
13834                 Collections.emptyList();
13835 
13836         final Matrix ba = generateBa();
13837         final double biasX = ba.getElementAtIndex(0);
13838         final double biasY = ba.getElementAtIndex(1);
13839         final double biasZ = ba.getElementAtIndex(2);
13840 
13841         final Matrix ma = generateMaCommonAxis();
13842         final double sx = ma.getElementAt(0, 0);
13843         final double sy = ma.getElementAt(1, 1);
13844         final double sz = ma.getElementAt(2, 2);
13845         final double mxy = ma.getElementAt(0, 1);
13846         final double mxz = ma.getElementAt(0, 2);
13847         final double myx = ma.getElementAt(1, 0);
13848         final double myz = ma.getElementAt(1, 2);
13849         final double mzx = ma.getElementAt(2, 0);
13850         final double mzy = ma.getElementAt(2, 1);
13851 
13852         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13853         final double latitude = Math.toRadians(
13854                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13855         final double longitude = Math.toRadians(
13856                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13857         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13858         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13859         final NEDVelocity nedVelocity = new NEDVelocity();
13860         final ECEFPosition ecefPosition = new ECEFPosition();
13861         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13862         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13863                 ecefPosition, ecefVelocity);
13864 
13865         final KnownPositionAccelerometerCalibrator calibrator =
13866                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13867                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13868                         myx, myz, mzx, mzy, this);
13869 
13870         // check default values
13871         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13872         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13873         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13874         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13875         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13876         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13877         final Acceleration bx2 = new Acceleration(0.0,
13878                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13879         calibrator.getInitialBiasXAsAcceleration(bx2);
13880         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13881         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13882         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13883         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13884         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13885         final Acceleration by2 = new Acceleration(0.0,
13886                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13887         calibrator.getInitialBiasYAsAcceleration(by2);
13888         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13889         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13890         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13891         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13892         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13893         final Acceleration bz2 = new Acceleration(0.0,
13894                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13895         calibrator.getInitialBiasZAsAcceleration(bz2);
13896         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13897         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13898         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13899         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13900         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13901         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
13902         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
13903         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
13904         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
13905         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
13906         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
13907         final double[] bias1 = calibrator.getInitialBias();
13908         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
13909         final double[] bias2 = new double[3];
13910         calibrator.getInitialBias(bias2);
13911         assertArrayEquals(bias1, bias2, 0.0);
13912         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13913         assertEquals(b1, ba);
13914         final Matrix b2 = new Matrix(3, 1);
13915         calibrator.getInitialBiasAsMatrix(b2);
13916         assertEquals(b1, b2);
13917         final Matrix ma1 = new Matrix(3, 3);
13918         ma1.setSubmatrix(0, 0,
13919                 2, 2,
13920                 new double[]{ sx, myx, mzx,
13921                         mxy, sy, mzy,
13922                         mxz, myz, sz});
13923         assertEquals(calibrator.getInitialMa(), ma1);
13924         final Matrix ma2 = new Matrix(3, 3);
13925         calibrator.getInitialMa(ma2);
13926         assertEquals(ma1, ma2);
13927         assertSame(calibrator.getEcefPosition(), ecefPosition);
13928         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13929         final NEDPosition nedPosition1 = new NEDPosition();
13930         assertTrue(calibrator.getNedPosition(nedPosition1));
13931         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13932         assertSame(calibrator.getMeasurements(), measurements);
13933         assertFalse(calibrator.isCommonAxisUsed());
13934         assertSame(calibrator.getListener(), this);
13935         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13936         assertFalse(calibrator.isReady());
13937         assertFalse(calibrator.isRunning());
13938         assertNull(calibrator.getEstimatedBiases());
13939         assertFalse(calibrator.getEstimatedBiases(null));
13940         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13941         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13942         assertNull(calibrator.getEstimatedBiasFx());
13943         assertNull(calibrator.getEstimatedBiasFy());
13944         assertNull(calibrator.getEstimatedBiasFz());
13945         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13946         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13947         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13948         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13949         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13950         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13951         assertNull(calibrator.getEstimatedMa());
13952         assertNull(calibrator.getEstimatedSx());
13953         assertNull(calibrator.getEstimatedSy());
13954         assertNull(calibrator.getEstimatedSz());
13955         assertNull(calibrator.getEstimatedMxy());
13956         assertNull(calibrator.getEstimatedMxz());
13957         assertNull(calibrator.getEstimatedMyx());
13958         assertNull(calibrator.getEstimatedMyz());
13959         assertNull(calibrator.getEstimatedMzx());
13960         assertNull(calibrator.getEstimatedMzy());
13961         assertNull(calibrator.getEstimatedCovariance());
13962         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13963     }
13964 
13965     @Test
13966     public void testConstructor121() throws WrongSizeException {
13967         final Matrix ba = generateBa();
13968         final double biasX = ba.getElementAtIndex(0);
13969         final double biasY = ba.getElementAtIndex(1);
13970         final double biasZ = ba.getElementAtIndex(2);
13971 
13972         final Matrix ma = generateMaCommonAxis();
13973         final double sx = ma.getElementAt(0, 0);
13974         final double sy = ma.getElementAt(1, 1);
13975         final double sz = ma.getElementAt(2, 2);
13976         final double mxy = ma.getElementAt(0, 1);
13977         final double mxz = ma.getElementAt(0, 2);
13978         final double myx = ma.getElementAt(1, 0);
13979         final double myz = ma.getElementAt(1, 2);
13980         final double mzx = ma.getElementAt(2, 0);
13981         final double mzy = ma.getElementAt(2, 1);
13982 
13983         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13984         final double latitude = Math.toRadians(
13985                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13986         final double longitude = Math.toRadians(
13987                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13988         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13989         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13990         final NEDVelocity nedVelocity = new NEDVelocity();
13991         final ECEFPosition ecefPosition = new ECEFPosition();
13992         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13993         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13994                 ecefPosition, ecefVelocity);
13995 
13996         final KnownPositionAccelerometerCalibrator calibrator =
13997                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13998                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
13999                         myx, myz, mzx, mzy);
14000 
14001         // check default values
14002         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14003         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14004         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14005         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14006         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14007         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14008         final Acceleration bx2 = new Acceleration(0.0,
14009                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14010         calibrator.getInitialBiasXAsAcceleration(bx2);
14011         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14012         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14013         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14014         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14015         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14016         final Acceleration by2 = new Acceleration(0.0,
14017                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14018         calibrator.getInitialBiasYAsAcceleration(by2);
14019         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14020         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14021         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14022         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14023         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14024         final Acceleration bz2 = new Acceleration(0.0,
14025                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14026         calibrator.getInitialBiasZAsAcceleration(bz2);
14027         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14028         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14029         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14030         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14031         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14032         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14033         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14034         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14035         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14036         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14037         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14038         final double[] bias1 = calibrator.getInitialBias();
14039         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14040         final double[] bias2 = new double[3];
14041         calibrator.getInitialBias(bias2);
14042         assertArrayEquals(bias1, bias2, 0.0);
14043         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14044         assertEquals(b1, ba);
14045         final Matrix b2 = new Matrix(3, 1);
14046         calibrator.getInitialBiasAsMatrix(b2);
14047         assertEquals(b1, b2);
14048         final Matrix ma1 = new Matrix(3, 3);
14049         ma1.setSubmatrix(0, 0,
14050                 2, 2,
14051                 new double[]{ sx, myx, mzx,
14052                         mxy, sy, mzy,
14053                         mxz, myz, sz});
14054         assertEquals(calibrator.getInitialMa(), ma1);
14055         final Matrix ma2 = new Matrix(3, 3);
14056         calibrator.getInitialMa(ma2);
14057         assertEquals(ma1, ma2);
14058         assertSame(calibrator.getEcefPosition(), ecefPosition);
14059         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14060         final NEDPosition nedPosition1 = new NEDPosition();
14061         assertTrue(calibrator.getNedPosition(nedPosition1));
14062         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14063         assertNull(calibrator.getMeasurements());
14064         assertTrue(calibrator.isCommonAxisUsed());
14065         assertNull(calibrator.getListener());
14066         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14067         assertFalse(calibrator.isReady());
14068         assertFalse(calibrator.isRunning());
14069         assertNull(calibrator.getEstimatedBiases());
14070         assertFalse(calibrator.getEstimatedBiases(null));
14071         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14072         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14073         assertNull(calibrator.getEstimatedBiasFx());
14074         assertNull(calibrator.getEstimatedBiasFy());
14075         assertNull(calibrator.getEstimatedBiasFz());
14076         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14077         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14078         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14079         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14080         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14081         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14082         assertNull(calibrator.getEstimatedMa());
14083         assertNull(calibrator.getEstimatedSx());
14084         assertNull(calibrator.getEstimatedSy());
14085         assertNull(calibrator.getEstimatedSz());
14086         assertNull(calibrator.getEstimatedMxy());
14087         assertNull(calibrator.getEstimatedMxz());
14088         assertNull(calibrator.getEstimatedMyx());
14089         assertNull(calibrator.getEstimatedMyz());
14090         assertNull(calibrator.getEstimatedMzx());
14091         assertNull(calibrator.getEstimatedMzy());
14092         assertNull(calibrator.getEstimatedCovariance());
14093         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14094     }
14095 
14096     @Test
14097     public void testConstructor122() throws WrongSizeException {
14098         final Matrix ba = generateBa();
14099         final double biasX = ba.getElementAtIndex(0);
14100         final double biasY = ba.getElementAtIndex(1);
14101         final double biasZ = ba.getElementAtIndex(2);
14102 
14103         final Matrix ma = generateMaCommonAxis();
14104         final double sx = ma.getElementAt(0, 0);
14105         final double sy = ma.getElementAt(1, 1);
14106         final double sz = ma.getElementAt(2, 2);
14107         final double mxy = ma.getElementAt(0, 1);
14108         final double mxz = ma.getElementAt(0, 2);
14109         final double myx = ma.getElementAt(1, 0);
14110         final double myz = ma.getElementAt(1, 2);
14111         final double mzx = ma.getElementAt(2, 0);
14112         final double mzy = ma.getElementAt(2, 1);
14113 
14114         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14115         final double latitude = Math.toRadians(
14116                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14117         final double longitude = Math.toRadians(
14118                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14119         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14120         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14121         final NEDVelocity nedVelocity = new NEDVelocity();
14122         final ECEFPosition ecefPosition = new ECEFPosition();
14123         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14124         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14125                 ecefPosition, ecefVelocity);
14126 
14127         final KnownPositionAccelerometerCalibrator calibrator =
14128                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14129                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14130                         myx, myz, mzx, mzy, this);
14131 
14132         // check default values
14133         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14134         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14135         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14136         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14137         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14138         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14139         final Acceleration bx2 = new Acceleration(0.0,
14140                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14141         calibrator.getInitialBiasXAsAcceleration(bx2);
14142         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14143         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14144         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14145         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14146         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14147         final Acceleration by2 = new Acceleration(0.0,
14148                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14149         calibrator.getInitialBiasYAsAcceleration(by2);
14150         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14151         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14152         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14153         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14154         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14155         final Acceleration bz2 = new Acceleration(0.0,
14156                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14157         calibrator.getInitialBiasZAsAcceleration(bz2);
14158         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14159         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14160         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14161         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14162         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14163         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14164         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14165         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14166         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14167         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14168         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14169         final double[] bias1 = calibrator.getInitialBias();
14170         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14171         final double[] bias2 = new double[3];
14172         calibrator.getInitialBias(bias2);
14173         assertArrayEquals(bias1, bias2, 0.0);
14174         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14175         assertEquals(b1, ba);
14176         final Matrix b2 = new Matrix(3, 1);
14177         calibrator.getInitialBiasAsMatrix(b2);
14178         assertEquals(b1, b2);
14179         final Matrix ma1 = new Matrix(3, 3);
14180         ma1.setSubmatrix(0, 0,
14181                 2, 2,
14182                 new double[]{ sx, myx, mzx,
14183                         mxy, sy, mzy,
14184                         mxz, myz, sz});
14185         assertEquals(calibrator.getInitialMa(), ma1);
14186         final Matrix ma2 = new Matrix(3, 3);
14187         calibrator.getInitialMa(ma2);
14188         assertEquals(ma1, ma2);
14189         assertSame(calibrator.getEcefPosition(), ecefPosition);
14190         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14191         final NEDPosition nedPosition1 = new NEDPosition();
14192         assertTrue(calibrator.getNedPosition(nedPosition1));
14193         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14194         assertNull(calibrator.getMeasurements());
14195         assertTrue(calibrator.isCommonAxisUsed());
14196         assertSame(calibrator.getListener(), this);
14197         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14198         assertFalse(calibrator.isReady());
14199         assertFalse(calibrator.isRunning());
14200         assertNull(calibrator.getEstimatedBiases());
14201         assertFalse(calibrator.getEstimatedBiases(null));
14202         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14203         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14204         assertNull(calibrator.getEstimatedBiasFx());
14205         assertNull(calibrator.getEstimatedBiasFy());
14206         assertNull(calibrator.getEstimatedBiasFz());
14207         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14208         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14209         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14210         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14211         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14212         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14213         assertNull(calibrator.getEstimatedMa());
14214         assertNull(calibrator.getEstimatedSx());
14215         assertNull(calibrator.getEstimatedSy());
14216         assertNull(calibrator.getEstimatedSz());
14217         assertNull(calibrator.getEstimatedMxy());
14218         assertNull(calibrator.getEstimatedMxz());
14219         assertNull(calibrator.getEstimatedMyx());
14220         assertNull(calibrator.getEstimatedMyz());
14221         assertNull(calibrator.getEstimatedMzx());
14222         assertNull(calibrator.getEstimatedMzy());
14223         assertNull(calibrator.getEstimatedCovariance());
14224         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14225     }
14226 
14227     @Test
14228     public void testConstructor123() throws WrongSizeException {
14229         final Collection<StandardDeviationBodyKinematics> measurements =
14230                 Collections.emptyList();
14231 
14232         final Matrix ba = generateBa();
14233         final double biasX = ba.getElementAtIndex(0);
14234         final double biasY = ba.getElementAtIndex(1);
14235         final double biasZ = ba.getElementAtIndex(2);
14236 
14237         final Matrix ma = generateMaCommonAxis();
14238         final double sx = ma.getElementAt(0, 0);
14239         final double sy = ma.getElementAt(1, 1);
14240         final double sz = ma.getElementAt(2, 2);
14241         final double mxy = ma.getElementAt(0, 1);
14242         final double mxz = ma.getElementAt(0, 2);
14243         final double myx = ma.getElementAt(1, 0);
14244         final double myz = ma.getElementAt(1, 2);
14245         final double mzx = ma.getElementAt(2, 0);
14246         final double mzy = ma.getElementAt(2, 1);
14247 
14248         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14249         final double latitude = Math.toRadians(
14250                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14251         final double longitude = Math.toRadians(
14252                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14253         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14254         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14255         final NEDVelocity nedVelocity = new NEDVelocity();
14256         final ECEFPosition ecefPosition = new ECEFPosition();
14257         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14258         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14259                 ecefPosition, ecefVelocity);
14260 
14261         final KnownPositionAccelerometerCalibrator calibrator =
14262                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14263                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14264                         myx, myz, mzx, mzy);
14265 
14266         // check default values
14267         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14268         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14269         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14270         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14271         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14272         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14273         final Acceleration bx2 = new Acceleration(0.0,
14274                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14275         calibrator.getInitialBiasXAsAcceleration(bx2);
14276         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14277         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14278         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14279         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14280         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14281         final Acceleration by2 = new Acceleration(0.0,
14282                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14283         calibrator.getInitialBiasYAsAcceleration(by2);
14284         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14285         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14286         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14287         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14288         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14289         final Acceleration bz2 = new Acceleration(0.0,
14290                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14291         calibrator.getInitialBiasZAsAcceleration(bz2);
14292         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14293         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14294         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14295         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14296         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14297         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14298         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14299         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14300         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14301         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14302         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14303         final double[] bias1 = calibrator.getInitialBias();
14304         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14305         final double[] bias2 = new double[3];
14306         calibrator.getInitialBias(bias2);
14307         assertArrayEquals(bias1, bias2, 0.0);
14308         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14309         assertEquals(b1, ba);
14310         final Matrix b2 = new Matrix(3, 1);
14311         calibrator.getInitialBiasAsMatrix(b2);
14312         assertEquals(b1, b2);
14313         final Matrix ma1 = new Matrix(3, 3);
14314         ma1.setSubmatrix(0, 0,
14315                 2, 2,
14316                 new double[]{ sx, myx, mzx,
14317                         mxy, sy, mzy,
14318                         mxz, myz, sz});
14319         assertEquals(calibrator.getInitialMa(), ma1);
14320         final Matrix ma2 = new Matrix(3, 3);
14321         calibrator.getInitialMa(ma2);
14322         assertEquals(ma1, ma2);
14323         assertSame(calibrator.getEcefPosition(), ecefPosition);
14324         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14325         final NEDPosition nedPosition1 = new NEDPosition();
14326         assertTrue(calibrator.getNedPosition(nedPosition1));
14327         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14328         assertSame(calibrator.getMeasurements(), measurements);
14329         assertTrue(calibrator.isCommonAxisUsed());
14330         assertNull(calibrator.getListener());
14331         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14332         assertFalse(calibrator.isReady());
14333         assertFalse(calibrator.isRunning());
14334         assertNull(calibrator.getEstimatedBiases());
14335         assertFalse(calibrator.getEstimatedBiases(null));
14336         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14337         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14338         assertNull(calibrator.getEstimatedBiasFx());
14339         assertNull(calibrator.getEstimatedBiasFy());
14340         assertNull(calibrator.getEstimatedBiasFz());
14341         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14342         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14343         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14344         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14345         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14346         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14347         assertNull(calibrator.getEstimatedMa());
14348         assertNull(calibrator.getEstimatedSx());
14349         assertNull(calibrator.getEstimatedSy());
14350         assertNull(calibrator.getEstimatedSz());
14351         assertNull(calibrator.getEstimatedMxy());
14352         assertNull(calibrator.getEstimatedMxz());
14353         assertNull(calibrator.getEstimatedMyx());
14354         assertNull(calibrator.getEstimatedMyz());
14355         assertNull(calibrator.getEstimatedMzx());
14356         assertNull(calibrator.getEstimatedMzy());
14357         assertNull(calibrator.getEstimatedCovariance());
14358         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14359     }
14360 
14361     @Test
14362     public void testConstructor124() throws WrongSizeException {
14363         final Collection<StandardDeviationBodyKinematics> measurements =
14364                 Collections.emptyList();
14365 
14366         final Matrix ba = generateBa();
14367         final double biasX = ba.getElementAtIndex(0);
14368         final double biasY = ba.getElementAtIndex(1);
14369         final double biasZ = ba.getElementAtIndex(2);
14370 
14371         final Matrix ma = generateMaCommonAxis();
14372         final double sx = ma.getElementAt(0, 0);
14373         final double sy = ma.getElementAt(1, 1);
14374         final double sz = ma.getElementAt(2, 2);
14375         final double mxy = ma.getElementAt(0, 1);
14376         final double mxz = ma.getElementAt(0, 2);
14377         final double myx = ma.getElementAt(1, 0);
14378         final double myz = ma.getElementAt(1, 2);
14379         final double mzx = ma.getElementAt(2, 0);
14380         final double mzy = ma.getElementAt(2, 1);
14381 
14382         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14383         final double latitude = Math.toRadians(
14384                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14385         final double longitude = Math.toRadians(
14386                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14387         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14388         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14389         final NEDVelocity nedVelocity = new NEDVelocity();
14390         final ECEFPosition ecefPosition = new ECEFPosition();
14391         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14392         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14393                 ecefPosition, ecefVelocity);
14394 
14395         final KnownPositionAccelerometerCalibrator calibrator =
14396                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14397                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14398                         myx, myz, mzx, mzy, this);
14399 
14400         // check default values
14401         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14402         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14403         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14404         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14405         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14406         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14407         final Acceleration bx2 = new Acceleration(0.0,
14408                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14409         calibrator.getInitialBiasXAsAcceleration(bx2);
14410         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14411         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14412         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14413         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14414         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14415         final Acceleration by2 = new Acceleration(0.0,
14416                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14417         calibrator.getInitialBiasYAsAcceleration(by2);
14418         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14419         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14420         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14421         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14422         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14423         final Acceleration bz2 = new Acceleration(0.0,
14424                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14425         calibrator.getInitialBiasZAsAcceleration(bz2);
14426         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14427         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14428         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14429         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14430         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14431         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14432         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14433         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14434         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14435         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14436         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14437         final double[] bias1 = calibrator.getInitialBias();
14438         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14439         final double[] bias2 = new double[3];
14440         calibrator.getInitialBias(bias2);
14441         assertArrayEquals(bias1, bias2, 0.0);
14442         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14443         assertEquals(b1, ba);
14444         final Matrix b2 = new Matrix(3, 1);
14445         calibrator.getInitialBiasAsMatrix(b2);
14446         assertEquals(b1, b2);
14447         final Matrix ma1 = new Matrix(3, 3);
14448         ma1.setSubmatrix(0, 0,
14449                 2, 2,
14450                 new double[]{ sx, myx, mzx,
14451                         mxy, sy, mzy,
14452                         mxz, myz, sz});
14453         assertEquals(calibrator.getInitialMa(), ma1);
14454         final Matrix ma2 = new Matrix(3, 3);
14455         calibrator.getInitialMa(ma2);
14456         assertEquals(ma1, ma2);
14457         assertSame(calibrator.getEcefPosition(), ecefPosition);
14458         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14459         final NEDPosition nedPosition1 = new NEDPosition();
14460         assertTrue(calibrator.getNedPosition(nedPosition1));
14461         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14462         assertSame(calibrator.getMeasurements(), measurements);
14463         assertTrue(calibrator.isCommonAxisUsed());
14464         assertSame(calibrator.getListener(), this);
14465         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14466         assertFalse(calibrator.isReady());
14467         assertFalse(calibrator.isRunning());
14468         assertNull(calibrator.getEstimatedBiases());
14469         assertFalse(calibrator.getEstimatedBiases(null));
14470         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14471         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14472         assertNull(calibrator.getEstimatedBiasFx());
14473         assertNull(calibrator.getEstimatedBiasFy());
14474         assertNull(calibrator.getEstimatedBiasFz());
14475         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14476         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14477         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14478         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14479         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14480         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14481         assertNull(calibrator.getEstimatedMa());
14482         assertNull(calibrator.getEstimatedSx());
14483         assertNull(calibrator.getEstimatedSy());
14484         assertNull(calibrator.getEstimatedSz());
14485         assertNull(calibrator.getEstimatedMxy());
14486         assertNull(calibrator.getEstimatedMxz());
14487         assertNull(calibrator.getEstimatedMyx());
14488         assertNull(calibrator.getEstimatedMyz());
14489         assertNull(calibrator.getEstimatedMzx());
14490         assertNull(calibrator.getEstimatedMzy());
14491         assertNull(calibrator.getEstimatedCovariance());
14492         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14493     }
14494 
14495     @Test
14496     public void testConstructor125() throws WrongSizeException {
14497         final Matrix ba = generateBa();
14498         final double biasX = ba.getElementAtIndex(0);
14499         final double biasY = ba.getElementAtIndex(1);
14500         final double biasZ = ba.getElementAtIndex(2);
14501 
14502         final Acceleration bx = new Acceleration(biasX,
14503                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14504         final Acceleration by = new Acceleration(biasY,
14505                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14506         final Acceleration bz = new Acceleration(biasZ,
14507                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14508 
14509         final Matrix ma = generateMaCommonAxis();
14510         final double sx = ma.getElementAt(0, 0);
14511         final double sy = ma.getElementAt(1, 1);
14512         final double sz = ma.getElementAt(2, 2);
14513         final double mxy = ma.getElementAt(0, 1);
14514         final double mxz = ma.getElementAt(0, 2);
14515         final double myx = ma.getElementAt(1, 0);
14516         final double myz = ma.getElementAt(1, 2);
14517         final double mzx = ma.getElementAt(2, 0);
14518         final double mzy = ma.getElementAt(2, 1);
14519 
14520         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14521         final double latitude = Math.toRadians(
14522                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14523         final double longitude = Math.toRadians(
14524                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14525         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14526         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14527         final NEDVelocity nedVelocity = new NEDVelocity();
14528         final ECEFPosition ecefPosition = new ECEFPosition();
14529         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14530         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14531                 ecefPosition, ecefVelocity);
14532 
14533         final KnownPositionAccelerometerCalibrator calibrator =
14534                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14535                         bx, by, bz, sx, sy, sz, mxy, mxz,
14536                         myx, myz, mzx, mzy);
14537 
14538         // check default values
14539         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14540         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14541         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14542         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14543         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14544         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14545         final Acceleration bx2 = new Acceleration(0.0,
14546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14547         calibrator.getInitialBiasXAsAcceleration(bx2);
14548         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14549         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14550         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14551         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14552         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14553         final Acceleration by2 = new Acceleration(0.0,
14554                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14555         calibrator.getInitialBiasYAsAcceleration(by2);
14556         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14557         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14558         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14559         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14560         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14561         final Acceleration bz2 = new Acceleration(0.0,
14562                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14563         calibrator.getInitialBiasZAsAcceleration(bz2);
14564         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14565         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14566         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14567         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14568         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14569         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14570         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14571         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14572         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14573         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14574         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14575         final double[] bias1 = calibrator.getInitialBias();
14576         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14577         final double[] bias2 = new double[3];
14578         calibrator.getInitialBias(bias2);
14579         assertArrayEquals(bias1, bias2, 0.0);
14580         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14581         assertEquals(b1, ba);
14582         final Matrix b2 = new Matrix(3, 1);
14583         calibrator.getInitialBiasAsMatrix(b2);
14584         assertEquals(b1, b2);
14585         final Matrix ma1 = new Matrix(3, 3);
14586         ma1.setSubmatrix(0, 0,
14587                 2, 2,
14588                 new double[]{ sx, myx, mzx,
14589                         mxy, sy, mzy,
14590                         mxz, myz, sz});
14591         assertEquals(calibrator.getInitialMa(), ma1);
14592         final Matrix ma2 = new Matrix(3, 3);
14593         calibrator.getInitialMa(ma2);
14594         assertEquals(ma1, ma2);
14595         assertSame(calibrator.getEcefPosition(), ecefPosition);
14596         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14597         final NEDPosition nedPosition1 = new NEDPosition();
14598         assertTrue(calibrator.getNedPosition(nedPosition1));
14599         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14600         assertNull(calibrator.getMeasurements());
14601         assertFalse(calibrator.isCommonAxisUsed());
14602         assertNull(calibrator.getListener());
14603         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14604         assertFalse(calibrator.isReady());
14605         assertFalse(calibrator.isRunning());
14606         assertNull(calibrator.getEstimatedBiases());
14607         assertFalse(calibrator.getEstimatedBiases(null));
14608         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14609         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14610         assertNull(calibrator.getEstimatedBiasFx());
14611         assertNull(calibrator.getEstimatedBiasFy());
14612         assertNull(calibrator.getEstimatedBiasFz());
14613         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14614         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14615         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14616         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14617         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14618         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14619         assertNull(calibrator.getEstimatedMa());
14620         assertNull(calibrator.getEstimatedSx());
14621         assertNull(calibrator.getEstimatedSy());
14622         assertNull(calibrator.getEstimatedSz());
14623         assertNull(calibrator.getEstimatedMxy());
14624         assertNull(calibrator.getEstimatedMxz());
14625         assertNull(calibrator.getEstimatedMyx());
14626         assertNull(calibrator.getEstimatedMyz());
14627         assertNull(calibrator.getEstimatedMzx());
14628         assertNull(calibrator.getEstimatedMzy());
14629         assertNull(calibrator.getEstimatedCovariance());
14630         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14631     }
14632 
14633     @Test
14634     public void testConstructor126() throws WrongSizeException {
14635         final Matrix ba = generateBa();
14636         final double biasX = ba.getElementAtIndex(0);
14637         final double biasY = ba.getElementAtIndex(1);
14638         final double biasZ = ba.getElementAtIndex(2);
14639 
14640         final Acceleration bx = new Acceleration(biasX,
14641                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14642         final Acceleration by = new Acceleration(biasY,
14643                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14644         final Acceleration bz = new Acceleration(biasZ,
14645                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14646 
14647         final Matrix ma = generateMaCommonAxis();
14648         final double sx = ma.getElementAt(0, 0);
14649         final double sy = ma.getElementAt(1, 1);
14650         final double sz = ma.getElementAt(2, 2);
14651         final double mxy = ma.getElementAt(0, 1);
14652         final double mxz = ma.getElementAt(0, 2);
14653         final double myx = ma.getElementAt(1, 0);
14654         final double myz = ma.getElementAt(1, 2);
14655         final double mzx = ma.getElementAt(2, 0);
14656         final double mzy = ma.getElementAt(2, 1);
14657 
14658         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14659         final double latitude = Math.toRadians(
14660                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14661         final double longitude = Math.toRadians(
14662                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14663         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14664         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14665         final NEDVelocity nedVelocity = new NEDVelocity();
14666         final ECEFPosition ecefPosition = new ECEFPosition();
14667         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14668         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14669                 ecefPosition, ecefVelocity);
14670 
14671         final KnownPositionAccelerometerCalibrator calibrator =
14672                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14673                         bx, by, bz, sx, sy, sz, mxy, mxz,
14674                         myx, myz, mzx, mzy, this);
14675 
14676         // check default values
14677         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14678         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14679         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14680         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14681         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14682         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14683         final Acceleration bx2 = new Acceleration(0.0,
14684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14685         calibrator.getInitialBiasXAsAcceleration(bx2);
14686         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14687         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14688         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14689         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14690         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14691         final Acceleration by2 = new Acceleration(0.0,
14692                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14693         calibrator.getInitialBiasYAsAcceleration(by2);
14694         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14695         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14696         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14697         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14698         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14699         final Acceleration bz2 = new Acceleration(0.0,
14700                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14701         calibrator.getInitialBiasZAsAcceleration(bz2);
14702         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14703         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14704         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14705         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14706         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14707         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14708         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14709         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14710         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14711         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14712         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14713         final double[] bias1 = calibrator.getInitialBias();
14714         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14715         final double[] bias2 = new double[3];
14716         calibrator.getInitialBias(bias2);
14717         assertArrayEquals(bias1, bias2, 0.0);
14718         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14719         assertEquals(b1, ba);
14720         final Matrix b2 = new Matrix(3, 1);
14721         calibrator.getInitialBiasAsMatrix(b2);
14722         assertEquals(b1, b2);
14723         final Matrix ma1 = new Matrix(3, 3);
14724         ma1.setSubmatrix(0, 0,
14725                 2, 2,
14726                 new double[]{ sx, myx, mzx,
14727                         mxy, sy, mzy,
14728                         mxz, myz, sz});
14729         assertEquals(calibrator.getInitialMa(), ma1);
14730         final Matrix ma2 = new Matrix(3, 3);
14731         calibrator.getInitialMa(ma2);
14732         assertEquals(ma1, ma2);
14733         assertSame(calibrator.getEcefPosition(), ecefPosition);
14734         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14735         final NEDPosition nedPosition1 = new NEDPosition();
14736         assertTrue(calibrator.getNedPosition(nedPosition1));
14737         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14738         assertNull(calibrator.getMeasurements());
14739         assertFalse(calibrator.isCommonAxisUsed());
14740         assertSame(calibrator.getListener(), this);
14741         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14742         assertFalse(calibrator.isReady());
14743         assertFalse(calibrator.isRunning());
14744         assertNull(calibrator.getEstimatedBiases());
14745         assertFalse(calibrator.getEstimatedBiases(null));
14746         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14747         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14748         assertNull(calibrator.getEstimatedBiasFx());
14749         assertNull(calibrator.getEstimatedBiasFy());
14750         assertNull(calibrator.getEstimatedBiasFz());
14751         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14752         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14753         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14754         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14755         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14756         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14757         assertNull(calibrator.getEstimatedMa());
14758         assertNull(calibrator.getEstimatedSx());
14759         assertNull(calibrator.getEstimatedSy());
14760         assertNull(calibrator.getEstimatedSz());
14761         assertNull(calibrator.getEstimatedMxy());
14762         assertNull(calibrator.getEstimatedMxz());
14763         assertNull(calibrator.getEstimatedMyx());
14764         assertNull(calibrator.getEstimatedMyz());
14765         assertNull(calibrator.getEstimatedMzx());
14766         assertNull(calibrator.getEstimatedMzy());
14767         assertNull(calibrator.getEstimatedCovariance());
14768         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14769     }
14770 
14771     @Test
14772     public void testConstructor127() throws WrongSizeException {
14773         final Collection<StandardDeviationBodyKinematics> measurements =
14774                 Collections.emptyList();
14775 
14776         final Matrix ba = generateBa();
14777         final double biasX = ba.getElementAtIndex(0);
14778         final double biasY = ba.getElementAtIndex(1);
14779         final double biasZ = ba.getElementAtIndex(2);
14780 
14781         final Acceleration bx = new Acceleration(biasX,
14782                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14783         final Acceleration by = new Acceleration(biasY,
14784                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14785         final Acceleration bz = new Acceleration(biasZ,
14786                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14787 
14788         final Matrix ma = generateMaCommonAxis();
14789         final double sx = ma.getElementAt(0, 0);
14790         final double sy = ma.getElementAt(1, 1);
14791         final double sz = ma.getElementAt(2, 2);
14792         final double mxy = ma.getElementAt(0, 1);
14793         final double mxz = ma.getElementAt(0, 2);
14794         final double myx = ma.getElementAt(1, 0);
14795         final double myz = ma.getElementAt(1, 2);
14796         final double mzx = ma.getElementAt(2, 0);
14797         final double mzy = ma.getElementAt(2, 1);
14798 
14799         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14800         final double latitude = Math.toRadians(
14801                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14802         final double longitude = Math.toRadians(
14803                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14804         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14805         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14806         final NEDVelocity nedVelocity = new NEDVelocity();
14807         final ECEFPosition ecefPosition = new ECEFPosition();
14808         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14809         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14810                 ecefPosition, ecefVelocity);
14811 
14812         final KnownPositionAccelerometerCalibrator calibrator =
14813                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14814                         bx, by, bz, sx, sy, sz, mxy, mxz,
14815                         myx, myz, mzx, mzy);
14816 
14817         // check default values
14818         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14819         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14820         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14821         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14822         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14823         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14824         final Acceleration bx2 = new Acceleration(0.0,
14825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14826         calibrator.getInitialBiasXAsAcceleration(bx2);
14827         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14828         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14829         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14830         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14831         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14832         final Acceleration by2 = new Acceleration(0.0,
14833                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14834         calibrator.getInitialBiasYAsAcceleration(by2);
14835         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14836         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14837         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14838         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14839         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14840         final Acceleration bz2 = new Acceleration(0.0,
14841                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14842         calibrator.getInitialBiasZAsAcceleration(bz2);
14843         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14844         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14845         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14846         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14847         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14848         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14849         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14850         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14851         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14852         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14853         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14854         final double[] bias1 = calibrator.getInitialBias();
14855         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14856         final double[] bias2 = new double[3];
14857         calibrator.getInitialBias(bias2);
14858         assertArrayEquals(bias1, bias2, 0.0);
14859         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14860         assertEquals(b1, ba);
14861         final Matrix b2 = new Matrix(3, 1);
14862         calibrator.getInitialBiasAsMatrix(b2);
14863         assertEquals(b1, b2);
14864         final Matrix ma1 = new Matrix(3, 3);
14865         ma1.setSubmatrix(0, 0,
14866                 2, 2,
14867                 new double[]{ sx, myx, mzx,
14868                         mxy, sy, mzy,
14869                         mxz, myz, sz});
14870         assertEquals(calibrator.getInitialMa(), ma1);
14871         final Matrix ma2 = new Matrix(3, 3);
14872         calibrator.getInitialMa(ma2);
14873         assertEquals(ma1, ma2);
14874         assertSame(calibrator.getEcefPosition(), ecefPosition);
14875         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14876         final NEDPosition nedPosition1 = new NEDPosition();
14877         assertTrue(calibrator.getNedPosition(nedPosition1));
14878         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14879         assertSame(calibrator.getMeasurements(), measurements);
14880         assertFalse(calibrator.isCommonAxisUsed());
14881         assertNull(calibrator.getListener());
14882         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14883         assertFalse(calibrator.isReady());
14884         assertFalse(calibrator.isRunning());
14885         assertNull(calibrator.getEstimatedBiases());
14886         assertFalse(calibrator.getEstimatedBiases(null));
14887         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14888         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14889         assertNull(calibrator.getEstimatedBiasFx());
14890         assertNull(calibrator.getEstimatedBiasFy());
14891         assertNull(calibrator.getEstimatedBiasFz());
14892         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14893         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14894         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14895         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14896         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14897         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14898         assertNull(calibrator.getEstimatedMa());
14899         assertNull(calibrator.getEstimatedSx());
14900         assertNull(calibrator.getEstimatedSy());
14901         assertNull(calibrator.getEstimatedSz());
14902         assertNull(calibrator.getEstimatedMxy());
14903         assertNull(calibrator.getEstimatedMxz());
14904         assertNull(calibrator.getEstimatedMyx());
14905         assertNull(calibrator.getEstimatedMyz());
14906         assertNull(calibrator.getEstimatedMzx());
14907         assertNull(calibrator.getEstimatedMzy());
14908         assertNull(calibrator.getEstimatedCovariance());
14909         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14910     }
14911 
14912     @Test
14913     public void testConstructor128() throws WrongSizeException {
14914         final Collection<StandardDeviationBodyKinematics> measurements =
14915                 Collections.emptyList();
14916 
14917         final Matrix ba = generateBa();
14918         final double biasX = ba.getElementAtIndex(0);
14919         final double biasY = ba.getElementAtIndex(1);
14920         final double biasZ = ba.getElementAtIndex(2);
14921 
14922         final Acceleration bx = new Acceleration(biasX,
14923                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14924         final Acceleration by = new Acceleration(biasY,
14925                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14926         final Acceleration bz = new Acceleration(biasZ,
14927                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14928 
14929         final Matrix ma = generateMaCommonAxis();
14930         final double sx = ma.getElementAt(0, 0);
14931         final double sy = ma.getElementAt(1, 1);
14932         final double sz = ma.getElementAt(2, 2);
14933         final double mxy = ma.getElementAt(0, 1);
14934         final double mxz = ma.getElementAt(0, 2);
14935         final double myx = ma.getElementAt(1, 0);
14936         final double myz = ma.getElementAt(1, 2);
14937         final double mzx = ma.getElementAt(2, 0);
14938         final double mzy = ma.getElementAt(2, 1);
14939 
14940         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14941         final double latitude = Math.toRadians(
14942                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14943         final double longitude = Math.toRadians(
14944                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14945         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14946         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14947         final NEDVelocity nedVelocity = new NEDVelocity();
14948         final ECEFPosition ecefPosition = new ECEFPosition();
14949         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14950         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14951                 ecefPosition, ecefVelocity);
14952 
14953         final KnownPositionAccelerometerCalibrator calibrator =
14954                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14955                         bx, by, bz, sx, sy, sz, mxy, mxz,
14956                         myx, myz, mzx, mzy, this);
14957 
14958         // check default values
14959         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14960         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14961         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14962         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14963         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14964         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14965         final Acceleration bx2 = new Acceleration(0.0,
14966                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14967         calibrator.getInitialBiasXAsAcceleration(bx2);
14968         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14969         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14970         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14971         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14972         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14973         final Acceleration by2 = new Acceleration(0.0,
14974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14975         calibrator.getInitialBiasYAsAcceleration(by2);
14976         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14977         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14978         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14979         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14980         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14981         final Acceleration bz2 = new Acceleration(0.0,
14982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14983         calibrator.getInitialBiasZAsAcceleration(bz2);
14984         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14985         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14986         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14987         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14988         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14989         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14990         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14991         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14992         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14993         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14994         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14995         final double[] bias1 = calibrator.getInitialBias();
14996         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
14997         final double[] bias2 = new double[3];
14998         calibrator.getInitialBias(bias2);
14999         assertArrayEquals(bias1, bias2, 0.0);
15000         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15001         assertEquals(b1, ba);
15002         final Matrix b2 = new Matrix(3, 1);
15003         calibrator.getInitialBiasAsMatrix(b2);
15004         assertEquals(b1, b2);
15005         final Matrix ma1 = new Matrix(3, 3);
15006         ma1.setSubmatrix(0, 0,
15007                 2, 2,
15008                 new double[]{ sx, myx, mzx,
15009                         mxy, sy, mzy,
15010                         mxz, myz, sz});
15011         assertEquals(calibrator.getInitialMa(), ma1);
15012         final Matrix ma2 = new Matrix(3, 3);
15013         calibrator.getInitialMa(ma2);
15014         assertEquals(ma1, ma2);
15015         assertSame(calibrator.getEcefPosition(), ecefPosition);
15016         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15017         final NEDPosition nedPosition1 = new NEDPosition();
15018         assertTrue(calibrator.getNedPosition(nedPosition1));
15019         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15020         assertSame(calibrator.getMeasurements(), measurements);
15021         assertFalse(calibrator.isCommonAxisUsed());
15022         assertSame(calibrator.getListener(), this);
15023         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15024         assertFalse(calibrator.isReady());
15025         assertFalse(calibrator.isRunning());
15026         assertNull(calibrator.getEstimatedBiases());
15027         assertFalse(calibrator.getEstimatedBiases(null));
15028         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15029         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15030         assertNull(calibrator.getEstimatedBiasFx());
15031         assertNull(calibrator.getEstimatedBiasFy());
15032         assertNull(calibrator.getEstimatedBiasFz());
15033         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15034         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15035         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15036         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15037         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15038         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15039         assertNull(calibrator.getEstimatedMa());
15040         assertNull(calibrator.getEstimatedSx());
15041         assertNull(calibrator.getEstimatedSy());
15042         assertNull(calibrator.getEstimatedSz());
15043         assertNull(calibrator.getEstimatedMxy());
15044         assertNull(calibrator.getEstimatedMxz());
15045         assertNull(calibrator.getEstimatedMyx());
15046         assertNull(calibrator.getEstimatedMyz());
15047         assertNull(calibrator.getEstimatedMzx());
15048         assertNull(calibrator.getEstimatedMzy());
15049         assertNull(calibrator.getEstimatedCovariance());
15050         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15051     }
15052 
15053     @Test
15054     public void testConstructor129() throws WrongSizeException {
15055         final Matrix ba = generateBa();
15056         final double biasX = ba.getElementAtIndex(0);
15057         final double biasY = ba.getElementAtIndex(1);
15058         final double biasZ = ba.getElementAtIndex(2);
15059 
15060         final Acceleration bx = new Acceleration(biasX,
15061                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15062         final Acceleration by = new Acceleration(biasY,
15063                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15064         final Acceleration bz = new Acceleration(biasZ,
15065                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15066 
15067         final Matrix ma = generateMaCommonAxis();
15068         final double sx = ma.getElementAt(0, 0);
15069         final double sy = ma.getElementAt(1, 1);
15070         final double sz = ma.getElementAt(2, 2);
15071         final double mxy = ma.getElementAt(0, 1);
15072         final double mxz = ma.getElementAt(0, 2);
15073         final double myx = ma.getElementAt(1, 0);
15074         final double myz = ma.getElementAt(1, 2);
15075         final double mzx = ma.getElementAt(2, 0);
15076         final double mzy = ma.getElementAt(2, 1);
15077 
15078         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15079         final double latitude = Math.toRadians(
15080                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15081         final double longitude = Math.toRadians(
15082                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15083         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15084         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15085         final NEDVelocity nedVelocity = new NEDVelocity();
15086         final ECEFPosition ecefPosition = new ECEFPosition();
15087         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15088         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15089                 ecefPosition, ecefVelocity);
15090 
15091         final KnownPositionAccelerometerCalibrator calibrator =
15092                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15093                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15094                         myx, myz, mzx, mzy);
15095 
15096         // check default values
15097         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15098         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15099         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15100         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15101         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15102         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15103         final Acceleration bx2 = new Acceleration(0.0,
15104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15105         calibrator.getInitialBiasXAsAcceleration(bx2);
15106         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15107         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15108         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15109         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15110         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15111         final Acceleration by2 = new Acceleration(0.0,
15112                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15113         calibrator.getInitialBiasYAsAcceleration(by2);
15114         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15115         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15116         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15117         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15118         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15119         final Acceleration bz2 = new Acceleration(0.0,
15120                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15121         calibrator.getInitialBiasZAsAcceleration(bz2);
15122         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15123         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15124         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15125         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15126         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15127         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15128         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15129         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15130         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15131         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15132         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15133         final double[] bias1 = calibrator.getInitialBias();
15134         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15135         final double[] bias2 = new double[3];
15136         calibrator.getInitialBias(bias2);
15137         assertArrayEquals(bias1, bias2, 0.0);
15138         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15139         assertEquals(b1, ba);
15140         final Matrix b2 = new Matrix(3, 1);
15141         calibrator.getInitialBiasAsMatrix(b2);
15142         assertEquals(b1, b2);
15143         final Matrix ma1 = new Matrix(3, 3);
15144         ma1.setSubmatrix(0, 0,
15145                 2, 2,
15146                 new double[]{ sx, myx, mzx,
15147                         mxy, sy, mzy,
15148                         mxz, myz, sz});
15149         assertEquals(calibrator.getInitialMa(), ma1);
15150         final Matrix ma2 = new Matrix(3, 3);
15151         calibrator.getInitialMa(ma2);
15152         assertEquals(ma1, ma2);
15153         assertSame(calibrator.getEcefPosition(), ecefPosition);
15154         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15155         final NEDPosition nedPosition1 = new NEDPosition();
15156         assertTrue(calibrator.getNedPosition(nedPosition1));
15157         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15158         assertNull(calibrator.getMeasurements());
15159         assertTrue(calibrator.isCommonAxisUsed());
15160         assertNull(calibrator.getListener());
15161         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15162         assertFalse(calibrator.isReady());
15163         assertFalse(calibrator.isRunning());
15164         assertNull(calibrator.getEstimatedBiases());
15165         assertFalse(calibrator.getEstimatedBiases(null));
15166         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15167         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15168         assertNull(calibrator.getEstimatedBiasFx());
15169         assertNull(calibrator.getEstimatedBiasFy());
15170         assertNull(calibrator.getEstimatedBiasFz());
15171         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15172         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15173         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15174         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15175         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15176         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15177         assertNull(calibrator.getEstimatedMa());
15178         assertNull(calibrator.getEstimatedSx());
15179         assertNull(calibrator.getEstimatedSy());
15180         assertNull(calibrator.getEstimatedSz());
15181         assertNull(calibrator.getEstimatedMxy());
15182         assertNull(calibrator.getEstimatedMxz());
15183         assertNull(calibrator.getEstimatedMyx());
15184         assertNull(calibrator.getEstimatedMyz());
15185         assertNull(calibrator.getEstimatedMzx());
15186         assertNull(calibrator.getEstimatedMzy());
15187         assertNull(calibrator.getEstimatedCovariance());
15188         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15189     }
15190 
15191     @Test
15192     public void testConstructor130() throws WrongSizeException {
15193         final Matrix ba = generateBa();
15194         final double biasX = ba.getElementAtIndex(0);
15195         final double biasY = ba.getElementAtIndex(1);
15196         final double biasZ = ba.getElementAtIndex(2);
15197 
15198         final Acceleration bx = new Acceleration(biasX,
15199                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15200         final Acceleration by = new Acceleration(biasY,
15201                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15202         final Acceleration bz = new Acceleration(biasZ,
15203                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15204 
15205         final Matrix ma = generateMaCommonAxis();
15206         final double sx = ma.getElementAt(0, 0);
15207         final double sy = ma.getElementAt(1, 1);
15208         final double sz = ma.getElementAt(2, 2);
15209         final double mxy = ma.getElementAt(0, 1);
15210         final double mxz = ma.getElementAt(0, 2);
15211         final double myx = ma.getElementAt(1, 0);
15212         final double myz = ma.getElementAt(1, 2);
15213         final double mzx = ma.getElementAt(2, 0);
15214         final double mzy = ma.getElementAt(2, 1);
15215 
15216         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15217         final double latitude = Math.toRadians(
15218                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15219         final double longitude = Math.toRadians(
15220                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15221         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15222         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15223         final NEDVelocity nedVelocity = new NEDVelocity();
15224         final ECEFPosition ecefPosition = new ECEFPosition();
15225         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15226         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15227                 ecefPosition, ecefVelocity);
15228 
15229         final KnownPositionAccelerometerCalibrator calibrator =
15230                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15231                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15232                         myx, myz, mzx, mzy, this);
15233 
15234         // check default values
15235         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15236         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15237         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15238         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15239         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15240         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15241         final Acceleration bx2 = new Acceleration(0.0,
15242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15243         calibrator.getInitialBiasXAsAcceleration(bx2);
15244         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15245         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15246         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15247         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15248         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15249         final Acceleration by2 = new Acceleration(0.0,
15250                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15251         calibrator.getInitialBiasYAsAcceleration(by2);
15252         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15253         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15254         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15255         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15256         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15257         final Acceleration bz2 = new Acceleration(0.0,
15258                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15259         calibrator.getInitialBiasZAsAcceleration(bz2);
15260         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15261         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15262         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15263         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15264         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15265         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15266         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15267         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15268         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15269         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15270         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15271         final double[] bias1 = calibrator.getInitialBias();
15272         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15273         final double[] bias2 = new double[3];
15274         calibrator.getInitialBias(bias2);
15275         assertArrayEquals(bias1, bias2, 0.0);
15276         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15277         assertEquals(b1, ba);
15278         final Matrix b2 = new Matrix(3, 1);
15279         calibrator.getInitialBiasAsMatrix(b2);
15280         assertEquals(b1, b2);
15281         final Matrix ma1 = new Matrix(3, 3);
15282         ma1.setSubmatrix(0, 0,
15283                 2, 2,
15284                 new double[]{ sx, myx, mzx,
15285                         mxy, sy, mzy,
15286                         mxz, myz, sz});
15287         assertEquals(calibrator.getInitialMa(), ma1);
15288         final Matrix ma2 = new Matrix(3, 3);
15289         calibrator.getInitialMa(ma2);
15290         assertEquals(ma1, ma2);
15291         assertSame(calibrator.getEcefPosition(), ecefPosition);
15292         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15293         final NEDPosition nedPosition1 = new NEDPosition();
15294         assertTrue(calibrator.getNedPosition(nedPosition1));
15295         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15296         assertNull(calibrator.getMeasurements());
15297         assertTrue(calibrator.isCommonAxisUsed());
15298         assertSame(calibrator.getListener(), this);
15299         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15300         assertFalse(calibrator.isReady());
15301         assertFalse(calibrator.isRunning());
15302         assertNull(calibrator.getEstimatedBiases());
15303         assertFalse(calibrator.getEstimatedBiases(null));
15304         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15305         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15306         assertNull(calibrator.getEstimatedBiasFx());
15307         assertNull(calibrator.getEstimatedBiasFy());
15308         assertNull(calibrator.getEstimatedBiasFz());
15309         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15310         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15311         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15312         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15313         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15314         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15315         assertNull(calibrator.getEstimatedMa());
15316         assertNull(calibrator.getEstimatedSx());
15317         assertNull(calibrator.getEstimatedSy());
15318         assertNull(calibrator.getEstimatedSz());
15319         assertNull(calibrator.getEstimatedMxy());
15320         assertNull(calibrator.getEstimatedMxz());
15321         assertNull(calibrator.getEstimatedMyx());
15322         assertNull(calibrator.getEstimatedMyz());
15323         assertNull(calibrator.getEstimatedMzx());
15324         assertNull(calibrator.getEstimatedMzy());
15325         assertNull(calibrator.getEstimatedCovariance());
15326         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15327     }
15328 
15329     @Test
15330     public void testConstructor131() throws WrongSizeException {
15331         final Collection<StandardDeviationBodyKinematics> measurements =
15332                 Collections.emptyList();
15333 
15334         final Matrix ba = generateBa();
15335         final double biasX = ba.getElementAtIndex(0);
15336         final double biasY = ba.getElementAtIndex(1);
15337         final double biasZ = ba.getElementAtIndex(2);
15338 
15339         final Acceleration bx = new Acceleration(biasX,
15340                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15341         final Acceleration by = new Acceleration(biasY,
15342                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15343         final Acceleration bz = new Acceleration(biasZ,
15344                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15345 
15346         final Matrix ma = generateMaCommonAxis();
15347         final double sx = ma.getElementAt(0, 0);
15348         final double sy = ma.getElementAt(1, 1);
15349         final double sz = ma.getElementAt(2, 2);
15350         final double mxy = ma.getElementAt(0, 1);
15351         final double mxz = ma.getElementAt(0, 2);
15352         final double myx = ma.getElementAt(1, 0);
15353         final double myz = ma.getElementAt(1, 2);
15354         final double mzx = ma.getElementAt(2, 0);
15355         final double mzy = ma.getElementAt(2, 1);
15356 
15357         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15358         final double latitude = Math.toRadians(
15359                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15360         final double longitude = Math.toRadians(
15361                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15362         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15363         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15364         final NEDVelocity nedVelocity = new NEDVelocity();
15365         final ECEFPosition ecefPosition = new ECEFPosition();
15366         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15367         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15368                 ecefPosition, ecefVelocity);
15369 
15370         final KnownPositionAccelerometerCalibrator calibrator =
15371                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15372                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15373                         myx, myz, mzx, mzy);
15374 
15375         // check default values
15376         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15377         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15378         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15379         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15380         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15381         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15382         final Acceleration bx2 = new Acceleration(0.0,
15383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15384         calibrator.getInitialBiasXAsAcceleration(bx2);
15385         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15386         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15387         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15388         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15389         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15390         final Acceleration by2 = new Acceleration(0.0,
15391                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15392         calibrator.getInitialBiasYAsAcceleration(by2);
15393         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15394         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15395         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15396         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15397         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15398         final Acceleration bz2 = new Acceleration(0.0,
15399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15400         calibrator.getInitialBiasZAsAcceleration(bz2);
15401         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15402         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15403         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15404         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15405         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15406         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15407         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15408         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15409         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15410         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15411         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15412         final double[] bias1 = calibrator.getInitialBias();
15413         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15414         final double[] bias2 = new double[3];
15415         calibrator.getInitialBias(bias2);
15416         assertArrayEquals(bias1, bias2, 0.0);
15417         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15418         assertEquals(b1, ba);
15419         final Matrix b2 = new Matrix(3, 1);
15420         calibrator.getInitialBiasAsMatrix(b2);
15421         assertEquals(b1, b2);
15422         final Matrix ma1 = new Matrix(3, 3);
15423         ma1.setSubmatrix(0, 0,
15424                 2, 2,
15425                 new double[]{ sx, myx, mzx,
15426                         mxy, sy, mzy,
15427                         mxz, myz, sz});
15428         assertEquals(calibrator.getInitialMa(), ma1);
15429         final Matrix ma2 = new Matrix(3, 3);
15430         calibrator.getInitialMa(ma2);
15431         assertEquals(ma1, ma2);
15432         assertSame(calibrator.getEcefPosition(), ecefPosition);
15433         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15434         final NEDPosition nedPosition1 = new NEDPosition();
15435         assertTrue(calibrator.getNedPosition(nedPosition1));
15436         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15437         assertSame(calibrator.getMeasurements(), measurements);
15438         assertTrue(calibrator.isCommonAxisUsed());
15439         assertNull(calibrator.getListener());
15440         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15441         assertFalse(calibrator.isReady());
15442         assertFalse(calibrator.isRunning());
15443         assertNull(calibrator.getEstimatedBiases());
15444         assertFalse(calibrator.getEstimatedBiases(null));
15445         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15446         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15447         assertNull(calibrator.getEstimatedBiasFx());
15448         assertNull(calibrator.getEstimatedBiasFy());
15449         assertNull(calibrator.getEstimatedBiasFz());
15450         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15451         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15452         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15453         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15454         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15455         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15456         assertNull(calibrator.getEstimatedMa());
15457         assertNull(calibrator.getEstimatedSx());
15458         assertNull(calibrator.getEstimatedSy());
15459         assertNull(calibrator.getEstimatedSz());
15460         assertNull(calibrator.getEstimatedMxy());
15461         assertNull(calibrator.getEstimatedMxz());
15462         assertNull(calibrator.getEstimatedMyx());
15463         assertNull(calibrator.getEstimatedMyz());
15464         assertNull(calibrator.getEstimatedMzx());
15465         assertNull(calibrator.getEstimatedMzy());
15466         assertNull(calibrator.getEstimatedCovariance());
15467         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15468     }
15469 
15470     @Test
15471     public void testConstructor132() throws WrongSizeException {
15472         final Collection<StandardDeviationBodyKinematics> measurements =
15473                 Collections.emptyList();
15474 
15475         final Matrix ba = generateBa();
15476         final double biasX = ba.getElementAtIndex(0);
15477         final double biasY = ba.getElementAtIndex(1);
15478         final double biasZ = ba.getElementAtIndex(2);
15479 
15480         final Acceleration bx = new Acceleration(biasX,
15481                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15482         final Acceleration by = new Acceleration(biasY,
15483                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15484         final Acceleration bz = new Acceleration(biasZ,
15485                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15486 
15487         final Matrix ma = generateMaCommonAxis();
15488         final double sx = ma.getElementAt(0, 0);
15489         final double sy = ma.getElementAt(1, 1);
15490         final double sz = ma.getElementAt(2, 2);
15491         final double mxy = ma.getElementAt(0, 1);
15492         final double mxz = ma.getElementAt(0, 2);
15493         final double myx = ma.getElementAt(1, 0);
15494         final double myz = ma.getElementAt(1, 2);
15495         final double mzx = ma.getElementAt(2, 0);
15496         final double mzy = ma.getElementAt(2, 1);
15497 
15498         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15499         final double latitude = Math.toRadians(
15500                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15501         final double longitude = Math.toRadians(
15502                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15503         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15504         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15505         final NEDVelocity nedVelocity = new NEDVelocity();
15506         final ECEFPosition ecefPosition = new ECEFPosition();
15507         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15508         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15509                 ecefPosition, ecefVelocity);
15510 
15511         final KnownPositionAccelerometerCalibrator calibrator =
15512                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15513                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15514                         myx, myz, mzx, mzy, this);
15515 
15516         // check default values
15517         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15518         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15519         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15520         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15521         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15522         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15523         final Acceleration bx2 = new Acceleration(0.0,
15524                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15525         calibrator.getInitialBiasXAsAcceleration(bx2);
15526         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15527         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15528         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15529         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15530         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15531         final Acceleration by2 = new Acceleration(0.0,
15532                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15533         calibrator.getInitialBiasYAsAcceleration(by2);
15534         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15535         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15536         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15537         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15538         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15539         final Acceleration bz2 = new Acceleration(0.0,
15540                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15541         calibrator.getInitialBiasZAsAcceleration(bz2);
15542         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15543         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15544         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15545         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15546         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15547         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15548         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15549         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15550         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15551         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15552         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15553         final double[] bias1 = calibrator.getInitialBias();
15554         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
15555         final double[] bias2 = new double[3];
15556         calibrator.getInitialBias(bias2);
15557         assertArrayEquals(bias1, bias2, 0.0);
15558         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15559         assertEquals(b1, ba);
15560         final Matrix b2 = new Matrix(3, 1);
15561         calibrator.getInitialBiasAsMatrix(b2);
15562         assertEquals(b1, b2);
15563         final Matrix ma1 = new Matrix(3, 3);
15564         ma1.setSubmatrix(0, 0,
15565                 2, 2,
15566                 new double[]{ sx, myx, mzx,
15567                         mxy, sy, mzy,
15568                         mxz, myz, sz});
15569         assertEquals(calibrator.getInitialMa(), ma1);
15570         final Matrix ma2 = new Matrix(3, 3);
15571         calibrator.getInitialMa(ma2);
15572         assertEquals(ma1, ma2);
15573         assertSame(calibrator.getEcefPosition(), ecefPosition);
15574         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15575         final NEDPosition nedPosition1 = new NEDPosition();
15576         assertTrue(calibrator.getNedPosition(nedPosition1));
15577         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15578         assertSame(calibrator.getMeasurements(), measurements);
15579         assertTrue(calibrator.isCommonAxisUsed());
15580         assertSame(calibrator.getListener(), this);
15581         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15582         assertFalse(calibrator.isReady());
15583         assertFalse(calibrator.isRunning());
15584         assertNull(calibrator.getEstimatedBiases());
15585         assertFalse(calibrator.getEstimatedBiases(null));
15586         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15587         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15588         assertNull(calibrator.getEstimatedBiasFx());
15589         assertNull(calibrator.getEstimatedBiasFy());
15590         assertNull(calibrator.getEstimatedBiasFz());
15591         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15592         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15593         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15594         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15595         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15596         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15597         assertNull(calibrator.getEstimatedMa());
15598         assertNull(calibrator.getEstimatedSx());
15599         assertNull(calibrator.getEstimatedSy());
15600         assertNull(calibrator.getEstimatedSz());
15601         assertNull(calibrator.getEstimatedMxy());
15602         assertNull(calibrator.getEstimatedMxz());
15603         assertNull(calibrator.getEstimatedMyx());
15604         assertNull(calibrator.getEstimatedMyz());
15605         assertNull(calibrator.getEstimatedMzx());
15606         assertNull(calibrator.getEstimatedMzy());
15607         assertNull(calibrator.getEstimatedCovariance());
15608         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15609     }
15610 
15611     @Test
15612     public void testConstructor133() throws WrongSizeException {
15613         final Matrix ba = generateBa();
15614         final double[] bias = ba.getBuffer();
15615         final double biasX = ba.getElementAtIndex(0);
15616         final double biasY = ba.getElementAtIndex(1);
15617         final double biasZ = ba.getElementAtIndex(2);
15618 
15619         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15620         final double latitude = Math.toRadians(
15621                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15622         final double longitude = Math.toRadians(
15623                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15624         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15625         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15626         final NEDVelocity nedVelocity = new NEDVelocity();
15627         final ECEFPosition ecefPosition = new ECEFPosition();
15628         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15629         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15630                 ecefPosition, ecefVelocity);
15631 
15632         KnownPositionAccelerometerCalibrator calibrator =
15633                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
15634 
15635         // check default values
15636         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15637         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15638         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15639         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15640         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15641         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15642         final Acceleration bx2 = new Acceleration(0.0,
15643                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15644         calibrator.getInitialBiasXAsAcceleration(bx2);
15645         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15646         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15647         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15648         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15649         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15650         final Acceleration by2 = new Acceleration(0.0,
15651                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15652         calibrator.getInitialBiasYAsAcceleration(by2);
15653         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15654         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15655         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15656         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15657         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15658         final Acceleration bz2 = new Acceleration(0.0,
15659                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15660         calibrator.getInitialBiasZAsAcceleration(bz2);
15661         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15662         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15663         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15664         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15665         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15666         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15667         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15668         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15669         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15670         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15671         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15672         final double[] bias1 = calibrator.getInitialBias();
15673         assertArrayEquals(bias1, bias, 0.0);
15674         final double[] bias2 = new double[3];
15675         calibrator.getInitialBias(bias2);
15676         assertArrayEquals(bias1, bias2, 0.0);
15677         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15678         assertEquals(b1, ba);
15679         final Matrix b2 = new Matrix(3, 1);
15680         calibrator.getInitialBiasAsMatrix(b2);
15681         assertEquals(b1, b2);
15682         final Matrix ma1 = calibrator.getInitialMa();
15683         assertEquals(ma1, new Matrix(3, 3));
15684         final Matrix ma2 = new Matrix(3, 3);
15685         calibrator.getInitialMa(ma2);
15686         assertEquals(ma1, ma2);
15687         assertSame(calibrator.getEcefPosition(), ecefPosition);
15688         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15689         final NEDPosition nedPosition1 = new NEDPosition();
15690         assertTrue(calibrator.getNedPosition(nedPosition1));
15691         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15692         assertNull(calibrator.getMeasurements());
15693         assertFalse(calibrator.isCommonAxisUsed());
15694         assertNull(calibrator.getListener());
15695         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15696         assertFalse(calibrator.isReady());
15697         assertFalse(calibrator.isRunning());
15698         assertNull(calibrator.getEstimatedBiases());
15699         assertFalse(calibrator.getEstimatedBiases(null));
15700         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15701         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15702         assertNull(calibrator.getEstimatedBiasFx());
15703         assertNull(calibrator.getEstimatedBiasFy());
15704         assertNull(calibrator.getEstimatedBiasFz());
15705         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15706         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15707         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15708         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15709         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15710         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15711         assertNull(calibrator.getEstimatedMa());
15712         assertNull(calibrator.getEstimatedSx());
15713         assertNull(calibrator.getEstimatedSy());
15714         assertNull(calibrator.getEstimatedSz());
15715         assertNull(calibrator.getEstimatedMxy());
15716         assertNull(calibrator.getEstimatedMxz());
15717         assertNull(calibrator.getEstimatedMyx());
15718         assertNull(calibrator.getEstimatedMyz());
15719         assertNull(calibrator.getEstimatedMzx());
15720         assertNull(calibrator.getEstimatedMzy());
15721         assertNull(calibrator.getEstimatedCovariance());
15722         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15723 
15724         // Force IllegalArgumentException
15725         calibrator = null;
15726         try {
15727             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15728                     new double[1]);
15729             fail("IllegalArgumentException expected but not thrown");
15730         } catch (final IllegalArgumentException ignore) {
15731         }
15732         assertNull(calibrator);
15733     }
15734 
15735     @Test
15736     public void testConstructor134() throws WrongSizeException {
15737         final Matrix ba = generateBa();
15738         final double[] bias = ba.getBuffer();
15739         final double biasX = ba.getElementAtIndex(0);
15740         final double biasY = ba.getElementAtIndex(1);
15741         final double biasZ = ba.getElementAtIndex(2);
15742 
15743         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15744         final double latitude = Math.toRadians(
15745                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15746         final double longitude = Math.toRadians(
15747                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15748         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15749         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15750         final NEDVelocity nedVelocity = new NEDVelocity();
15751         final ECEFPosition ecefPosition = new ECEFPosition();
15752         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15753         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15754                 ecefPosition, ecefVelocity);
15755 
15756         KnownPositionAccelerometerCalibrator calibrator =
15757                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
15758                         this);
15759 
15760         // check default values
15761         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15762         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15763         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15764         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15765         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15766         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15767         final Acceleration bx2 = new Acceleration(0.0,
15768                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15769         calibrator.getInitialBiasXAsAcceleration(bx2);
15770         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15771         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15772         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15773         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15774         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15775         final Acceleration by2 = new Acceleration(0.0,
15776                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15777         calibrator.getInitialBiasYAsAcceleration(by2);
15778         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15779         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15780         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15781         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15782         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15783         final Acceleration bz2 = new Acceleration(0.0,
15784                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15785         calibrator.getInitialBiasZAsAcceleration(bz2);
15786         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15787         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15788         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15789         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15790         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15791         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15792         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15793         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15794         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15795         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15796         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15797         final double[] bias1 = calibrator.getInitialBias();
15798         assertArrayEquals(bias1, bias, 0.0);
15799         final double[] bias2 = new double[3];
15800         calibrator.getInitialBias(bias2);
15801         assertArrayEquals(bias1, bias2, 0.0);
15802         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15803         assertEquals(b1, ba);
15804         final Matrix b2 = new Matrix(3, 1);
15805         calibrator.getInitialBiasAsMatrix(b2);
15806         assertEquals(b1, b2);
15807         final Matrix ma1 = calibrator.getInitialMa();
15808         assertEquals(ma1, new Matrix(3, 3));
15809         final Matrix ma2 = new Matrix(3, 3);
15810         calibrator.getInitialMa(ma2);
15811         assertEquals(ma1, ma2);
15812         assertSame(calibrator.getEcefPosition(), ecefPosition);
15813         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15814         final NEDPosition nedPosition1 = new NEDPosition();
15815         assertTrue(calibrator.getNedPosition(nedPosition1));
15816         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15817         assertNull(calibrator.getMeasurements());
15818         assertFalse(calibrator.isCommonAxisUsed());
15819         assertSame(calibrator.getListener(), this);
15820         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15821         assertFalse(calibrator.isReady());
15822         assertFalse(calibrator.isRunning());
15823         assertNull(calibrator.getEstimatedBiases());
15824         assertFalse(calibrator.getEstimatedBiases(null));
15825         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15826         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15827         assertNull(calibrator.getEstimatedBiasFx());
15828         assertNull(calibrator.getEstimatedBiasFy());
15829         assertNull(calibrator.getEstimatedBiasFz());
15830         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15831         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15832         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15833         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15834         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15835         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15836         assertNull(calibrator.getEstimatedMa());
15837         assertNull(calibrator.getEstimatedSx());
15838         assertNull(calibrator.getEstimatedSy());
15839         assertNull(calibrator.getEstimatedSz());
15840         assertNull(calibrator.getEstimatedMxy());
15841         assertNull(calibrator.getEstimatedMxz());
15842         assertNull(calibrator.getEstimatedMyx());
15843         assertNull(calibrator.getEstimatedMyz());
15844         assertNull(calibrator.getEstimatedMzx());
15845         assertNull(calibrator.getEstimatedMzy());
15846         assertNull(calibrator.getEstimatedCovariance());
15847         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15848 
15849         // Force IllegalArgumentException
15850         calibrator = null;
15851         try {
15852             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15853                     new double[1], this);
15854             fail("IllegalArgumentException expected but not thrown");
15855         } catch (final IllegalArgumentException ignore) {
15856         }
15857         assertNull(calibrator);
15858     }
15859 
15860     @Test
15861     public void testConstructor135() throws WrongSizeException {
15862         final Collection<StandardDeviationBodyKinematics> measurements =
15863                 Collections.emptyList();
15864 
15865         final Matrix ba = generateBa();
15866         final double[] bias = ba.getBuffer();
15867         final double biasX = ba.getElementAtIndex(0);
15868         final double biasY = ba.getElementAtIndex(1);
15869         final double biasZ = ba.getElementAtIndex(2);
15870 
15871         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15872         final double latitude = Math.toRadians(
15873                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15874         final double longitude = Math.toRadians(
15875                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15876         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15877         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15878         final NEDVelocity nedVelocity = new NEDVelocity();
15879         final ECEFPosition ecefPosition = new ECEFPosition();
15880         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15881         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15882                 ecefPosition, ecefVelocity);
15883 
15884         KnownPositionAccelerometerCalibrator calibrator =
15885                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15886                         measurements, bias);
15887 
15888         // check default values
15889         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15890         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15891         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15892         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15893         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15894         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15895         final Acceleration bx2 = new Acceleration(0.0,
15896                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15897         calibrator.getInitialBiasXAsAcceleration(bx2);
15898         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15899         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15900         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15901         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15902         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15903         final Acceleration by2 = new Acceleration(0.0,
15904                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15905         calibrator.getInitialBiasYAsAcceleration(by2);
15906         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15907         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15908         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15909         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15910         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15911         final Acceleration bz2 = new Acceleration(0.0,
15912                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15913         calibrator.getInitialBiasZAsAcceleration(bz2);
15914         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15915         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15916         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
15917         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
15918         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
15919         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
15920         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
15921         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
15922         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
15923         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
15924         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
15925         final double[] bias1 = calibrator.getInitialBias();
15926         assertArrayEquals(bias1, bias, 0.0);
15927         final double[] bias2 = new double[3];
15928         calibrator.getInitialBias(bias2);
15929         assertArrayEquals(bias1, bias2, 0.0);
15930         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15931         assertEquals(b1, ba);
15932         final Matrix b2 = new Matrix(3, 1);
15933         calibrator.getInitialBiasAsMatrix(b2);
15934         assertEquals(b1, b2);
15935         final Matrix ma1 = calibrator.getInitialMa();
15936         assertEquals(ma1, new Matrix(3, 3));
15937         final Matrix ma2 = new Matrix(3, 3);
15938         calibrator.getInitialMa(ma2);
15939         assertEquals(ma1, ma2);
15940         assertSame(calibrator.getEcefPosition(), ecefPosition);
15941         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15942         final NEDPosition nedPosition1 = new NEDPosition();
15943         assertTrue(calibrator.getNedPosition(nedPosition1));
15944         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15945         assertSame(calibrator.getMeasurements(), measurements);
15946         assertFalse(calibrator.isCommonAxisUsed());
15947         assertNull(calibrator.getListener());
15948         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15949         assertFalse(calibrator.isReady());
15950         assertFalse(calibrator.isRunning());
15951         assertNull(calibrator.getEstimatedBiases());
15952         assertFalse(calibrator.getEstimatedBiases(null));
15953         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15954         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15955         assertNull(calibrator.getEstimatedBiasFx());
15956         assertNull(calibrator.getEstimatedBiasFy());
15957         assertNull(calibrator.getEstimatedBiasFz());
15958         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15959         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15960         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15961         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15962         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15963         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15964         assertNull(calibrator.getEstimatedMa());
15965         assertNull(calibrator.getEstimatedSx());
15966         assertNull(calibrator.getEstimatedSy());
15967         assertNull(calibrator.getEstimatedSz());
15968         assertNull(calibrator.getEstimatedMxy());
15969         assertNull(calibrator.getEstimatedMxz());
15970         assertNull(calibrator.getEstimatedMyx());
15971         assertNull(calibrator.getEstimatedMyz());
15972         assertNull(calibrator.getEstimatedMzx());
15973         assertNull(calibrator.getEstimatedMzy());
15974         assertNull(calibrator.getEstimatedCovariance());
15975         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15976 
15977         // Force IllegalArgumentException
15978         calibrator = null;
15979         try {
15980             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
15981                     measurements, new double[1]);
15982             fail("IllegalArgumentException expected but not thrown");
15983         } catch (final IllegalArgumentException ignore) {
15984         }
15985         assertNull(calibrator);
15986     }
15987 
15988     @Test
15989     public void testConstructor136() throws WrongSizeException {
15990         final Collection<StandardDeviationBodyKinematics> measurements =
15991                 Collections.emptyList();
15992 
15993         final Matrix ba = generateBa();
15994         final double[] bias = ba.getBuffer();
15995         final double biasX = ba.getElementAtIndex(0);
15996         final double biasY = ba.getElementAtIndex(1);
15997         final double biasZ = ba.getElementAtIndex(2);
15998 
15999         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16000         final double latitude = Math.toRadians(
16001                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16002         final double longitude = Math.toRadians(
16003                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16004         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16005         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16006         final NEDVelocity nedVelocity = new NEDVelocity();
16007         final ECEFPosition ecefPosition = new ECEFPosition();
16008         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16009         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16010                 ecefPosition, ecefVelocity);
16011 
16012         KnownPositionAccelerometerCalibrator calibrator =
16013                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16014                         measurements, bias, this);
16015 
16016         // check default values
16017         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16018         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16019         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16020         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16021         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16022         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16023         final Acceleration bx2 = new Acceleration(0.0,
16024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16025         calibrator.getInitialBiasXAsAcceleration(bx2);
16026         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16027         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16028         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16029         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16030         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16031         final Acceleration by2 = new Acceleration(0.0,
16032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16033         calibrator.getInitialBiasYAsAcceleration(by2);
16034         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16035         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16036         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16037         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16038         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16039         final Acceleration bz2 = new Acceleration(0.0,
16040                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16041         calibrator.getInitialBiasZAsAcceleration(bz2);
16042         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16043         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16044         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16045         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16046         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16047         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16048         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16049         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16050         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16051         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16052         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16053         final double[] bias1 = calibrator.getInitialBias();
16054         assertArrayEquals(bias1, bias, 0.0);
16055         final double[] bias2 = new double[3];
16056         calibrator.getInitialBias(bias2);
16057         assertArrayEquals(bias1, bias2, 0.0);
16058         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16059         assertEquals(b1, ba);
16060         final Matrix b2 = new Matrix(3, 1);
16061         calibrator.getInitialBiasAsMatrix(b2);
16062         assertEquals(b1, b2);
16063         final Matrix ma1 = calibrator.getInitialMa();
16064         assertEquals(ma1, new Matrix(3, 3));
16065         final Matrix ma2 = new Matrix(3, 3);
16066         calibrator.getInitialMa(ma2);
16067         assertEquals(ma1, ma2);
16068         assertSame(calibrator.getEcefPosition(), ecefPosition);
16069         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16070         final NEDPosition nedPosition1 = new NEDPosition();
16071         assertTrue(calibrator.getNedPosition(nedPosition1));
16072         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16073         assertSame(calibrator.getMeasurements(), measurements);
16074         assertFalse(calibrator.isCommonAxisUsed());
16075         assertSame(calibrator.getListener(), this);
16076         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16077         assertFalse(calibrator.isReady());
16078         assertFalse(calibrator.isRunning());
16079         assertNull(calibrator.getEstimatedBiases());
16080         assertFalse(calibrator.getEstimatedBiases(null));
16081         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16082         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16083         assertNull(calibrator.getEstimatedBiasFx());
16084         assertNull(calibrator.getEstimatedBiasFy());
16085         assertNull(calibrator.getEstimatedBiasFz());
16086         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16087         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16088         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16089         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16090         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16091         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16092         assertNull(calibrator.getEstimatedMa());
16093         assertNull(calibrator.getEstimatedSx());
16094         assertNull(calibrator.getEstimatedSy());
16095         assertNull(calibrator.getEstimatedSz());
16096         assertNull(calibrator.getEstimatedMxy());
16097         assertNull(calibrator.getEstimatedMxz());
16098         assertNull(calibrator.getEstimatedMyx());
16099         assertNull(calibrator.getEstimatedMyz());
16100         assertNull(calibrator.getEstimatedMzx());
16101         assertNull(calibrator.getEstimatedMzy());
16102         assertNull(calibrator.getEstimatedCovariance());
16103         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16104 
16105         // Force IllegalArgumentException
16106         calibrator = null;
16107         try {
16108             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16109                     measurements, new double[1], this);
16110             fail("IllegalArgumentException expected but not thrown");
16111         } catch (final IllegalArgumentException ignore) {
16112         }
16113         assertNull(calibrator);
16114     }
16115 
16116     @Test
16117     public void testConstructor137() throws WrongSizeException {
16118         final Matrix ba = generateBa();
16119         final double[] bias = ba.getBuffer();
16120         final double biasX = ba.getElementAtIndex(0);
16121         final double biasY = ba.getElementAtIndex(1);
16122         final double biasZ = ba.getElementAtIndex(2);
16123 
16124         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16125         final double latitude = Math.toRadians(
16126                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16127         final double longitude = Math.toRadians(
16128                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16129         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16130         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16131         final NEDVelocity nedVelocity = new NEDVelocity();
16132         final ECEFPosition ecefPosition = new ECEFPosition();
16133         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16134         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16135                 ecefPosition, ecefVelocity);
16136 
16137         KnownPositionAccelerometerCalibrator calibrator =
16138                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16139                         true, bias);
16140 
16141         // check default values
16142         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16143         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16144         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16145         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16146         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16147         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16148         final Acceleration bx2 = new Acceleration(0.0,
16149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16150         calibrator.getInitialBiasXAsAcceleration(bx2);
16151         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16152         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16153         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16154         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16155         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16156         final Acceleration by2 = new Acceleration(0.0,
16157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16158         calibrator.getInitialBiasYAsAcceleration(by2);
16159         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16160         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16161         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16162         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16163         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16164         final Acceleration bz2 = new Acceleration(0.0,
16165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16166         calibrator.getInitialBiasZAsAcceleration(bz2);
16167         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16168         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16169         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16170         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16171         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16172         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16173         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16174         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16175         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16176         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16177         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16178         final double[] bias1 = calibrator.getInitialBias();
16179         assertArrayEquals(bias1, bias, 0.0);
16180         final double[] bias2 = new double[3];
16181         calibrator.getInitialBias(bias2);
16182         assertArrayEquals(bias1, bias2, 0.0);
16183         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16184         assertEquals(b1, ba);
16185         final Matrix b2 = new Matrix(3, 1);
16186         calibrator.getInitialBiasAsMatrix(b2);
16187         assertEquals(b1, b2);
16188         final Matrix ma1 = calibrator.getInitialMa();
16189         assertEquals(ma1, new Matrix(3, 3));
16190         final Matrix ma2 = new Matrix(3, 3);
16191         calibrator.getInitialMa(ma2);
16192         assertEquals(ma1, ma2);
16193         assertSame(calibrator.getEcefPosition(), ecefPosition);
16194         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16195         final NEDPosition nedPosition1 = new NEDPosition();
16196         assertTrue(calibrator.getNedPosition(nedPosition1));
16197         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16198         assertNull(calibrator.getMeasurements());
16199         assertTrue(calibrator.isCommonAxisUsed());
16200         assertNull(calibrator.getListener());
16201         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16202         assertFalse(calibrator.isReady());
16203         assertFalse(calibrator.isRunning());
16204         assertNull(calibrator.getEstimatedBiases());
16205         assertFalse(calibrator.getEstimatedBiases(null));
16206         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16207         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16208         assertNull(calibrator.getEstimatedBiasFx());
16209         assertNull(calibrator.getEstimatedBiasFy());
16210         assertNull(calibrator.getEstimatedBiasFz());
16211         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16212         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16213         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16214         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16215         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16216         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16217         assertNull(calibrator.getEstimatedMa());
16218         assertNull(calibrator.getEstimatedSx());
16219         assertNull(calibrator.getEstimatedSy());
16220         assertNull(calibrator.getEstimatedSz());
16221         assertNull(calibrator.getEstimatedMxy());
16222         assertNull(calibrator.getEstimatedMxz());
16223         assertNull(calibrator.getEstimatedMyx());
16224         assertNull(calibrator.getEstimatedMyz());
16225         assertNull(calibrator.getEstimatedMzx());
16226         assertNull(calibrator.getEstimatedMzy());
16227         assertNull(calibrator.getEstimatedCovariance());
16228         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16229 
16230         // Force IllegalArgumentException
16231         calibrator = null;
16232         try {
16233             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16234                     true, new double[1]);
16235             fail("IllegalArgumentException expected but not thrown");
16236         } catch (final IllegalArgumentException ignore) {
16237         }
16238         assertNull(calibrator);
16239     }
16240 
16241     @Test
16242     public void testConstructor138() throws WrongSizeException {
16243         final Matrix ba = generateBa();
16244         final double[] bias = ba.getBuffer();
16245         final double biasX = ba.getElementAtIndex(0);
16246         final double biasY = ba.getElementAtIndex(1);
16247         final double biasZ = ba.getElementAtIndex(2);
16248 
16249         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16250         final double latitude = Math.toRadians(
16251                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16252         final double longitude = Math.toRadians(
16253                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16254         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16255         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16256         final NEDVelocity nedVelocity = new NEDVelocity();
16257         final ECEFPosition ecefPosition = new ECEFPosition();
16258         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16259         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16260                 ecefPosition, ecefVelocity);
16261 
16262         KnownPositionAccelerometerCalibrator calibrator =
16263                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16264                         true, bias, this);
16265 
16266         // check default values
16267         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16268         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16269         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16270         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16271         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16272         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16273         final Acceleration bx2 = new Acceleration(0.0,
16274                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16275         calibrator.getInitialBiasXAsAcceleration(bx2);
16276         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16277         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16278         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16279         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16280         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16281         final Acceleration by2 = new Acceleration(0.0,
16282                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16283         calibrator.getInitialBiasYAsAcceleration(by2);
16284         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16285         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16286         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16287         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16288         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16289         final Acceleration bz2 = new Acceleration(0.0,
16290                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16291         calibrator.getInitialBiasZAsAcceleration(bz2);
16292         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16293         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16294         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16295         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16296         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16297         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16298         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16299         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16300         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16301         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16302         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16303         final double[] bias1 = calibrator.getInitialBias();
16304         assertArrayEquals(bias1, bias, 0.0);
16305         final double[] bias2 = new double[3];
16306         calibrator.getInitialBias(bias2);
16307         assertArrayEquals(bias1, bias2, 0.0);
16308         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16309         assertEquals(b1, ba);
16310         final Matrix b2 = new Matrix(3, 1);
16311         calibrator.getInitialBiasAsMatrix(b2);
16312         assertEquals(b1, b2);
16313         final Matrix ma1 = calibrator.getInitialMa();
16314         assertEquals(ma1, new Matrix(3, 3));
16315         final Matrix ma2 = new Matrix(3, 3);
16316         calibrator.getInitialMa(ma2);
16317         assertEquals(ma1, ma2);
16318         assertSame(calibrator.getEcefPosition(), ecefPosition);
16319         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16320         final NEDPosition nedPosition1 = new NEDPosition();
16321         assertTrue(calibrator.getNedPosition(nedPosition1));
16322         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16323         assertNull(calibrator.getMeasurements());
16324         assertTrue(calibrator.isCommonAxisUsed());
16325         assertSame(calibrator.getListener(), this);
16326         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16327         assertFalse(calibrator.isReady());
16328         assertFalse(calibrator.isRunning());
16329         assertNull(calibrator.getEstimatedBiases());
16330         assertFalse(calibrator.getEstimatedBiases(null));
16331         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16332         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16333         assertNull(calibrator.getEstimatedBiasFx());
16334         assertNull(calibrator.getEstimatedBiasFy());
16335         assertNull(calibrator.getEstimatedBiasFz());
16336         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16337         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16338         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16339         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16340         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16341         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16342         assertNull(calibrator.getEstimatedMa());
16343         assertNull(calibrator.getEstimatedSx());
16344         assertNull(calibrator.getEstimatedSy());
16345         assertNull(calibrator.getEstimatedSz());
16346         assertNull(calibrator.getEstimatedMxy());
16347         assertNull(calibrator.getEstimatedMxz());
16348         assertNull(calibrator.getEstimatedMyx());
16349         assertNull(calibrator.getEstimatedMyz());
16350         assertNull(calibrator.getEstimatedMzx());
16351         assertNull(calibrator.getEstimatedMzy());
16352         assertNull(calibrator.getEstimatedCovariance());
16353         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16354 
16355         // Force IllegalArgumentException
16356         calibrator = null;
16357         try {
16358             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16359                     true, new double[1], this);
16360             fail("IllegalArgumentException expected but not thrown");
16361         } catch (final IllegalArgumentException ignore) {
16362         }
16363         assertNull(calibrator);
16364     }
16365 
16366     @Test
16367     public void testConstructor139() throws WrongSizeException {
16368         final Collection<StandardDeviationBodyKinematics> measurements =
16369                 Collections.emptyList();
16370 
16371         final Matrix ba = generateBa();
16372         final double[] bias = ba.getBuffer();
16373         final double biasX = ba.getElementAtIndex(0);
16374         final double biasY = ba.getElementAtIndex(1);
16375         final double biasZ = ba.getElementAtIndex(2);
16376 
16377         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16378         final double latitude = Math.toRadians(
16379                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16380         final double longitude = Math.toRadians(
16381                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16382         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16383         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16384         final NEDVelocity nedVelocity = new NEDVelocity();
16385         final ECEFPosition ecefPosition = new ECEFPosition();
16386         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16387         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16388                 ecefPosition, ecefVelocity);
16389 
16390         KnownPositionAccelerometerCalibrator calibrator =
16391                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16392                         true, bias);
16393 
16394         // check default values
16395         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16396         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16397         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16398         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16399         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16400         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16401         final Acceleration bx2 = new Acceleration(0.0,
16402                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16403         calibrator.getInitialBiasXAsAcceleration(bx2);
16404         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16405         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16406         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16407         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16408         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16409         final Acceleration by2 = new Acceleration(0.0,
16410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16411         calibrator.getInitialBiasYAsAcceleration(by2);
16412         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16413         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16414         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16415         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16416         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16417         final Acceleration bz2 = new Acceleration(0.0,
16418                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16419         calibrator.getInitialBiasZAsAcceleration(bz2);
16420         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16421         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16422         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16423         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16424         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16425         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16426         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16427         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16428         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16429         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16430         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16431         final double[] bias1 = calibrator.getInitialBias();
16432         assertArrayEquals(bias1, bias, 0.0);
16433         final double[] bias2 = new double[3];
16434         calibrator.getInitialBias(bias2);
16435         assertArrayEquals(bias1, bias2, 0.0);
16436         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16437         assertEquals(b1, ba);
16438         final Matrix b2 = new Matrix(3, 1);
16439         calibrator.getInitialBiasAsMatrix(b2);
16440         assertEquals(b1, b2);
16441         final Matrix ma1 = calibrator.getInitialMa();
16442         assertEquals(ma1, new Matrix(3, 3));
16443         final Matrix ma2 = new Matrix(3, 3);
16444         calibrator.getInitialMa(ma2);
16445         assertEquals(ma1, ma2);
16446         assertSame(calibrator.getEcefPosition(), ecefPosition);
16447         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16448         final NEDPosition nedPosition1 = new NEDPosition();
16449         assertTrue(calibrator.getNedPosition(nedPosition1));
16450         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16451         assertSame(calibrator.getMeasurements(), measurements);
16452         assertTrue(calibrator.isCommonAxisUsed());
16453         assertNull(calibrator.getListener());
16454         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16455         assertFalse(calibrator.isReady());
16456         assertFalse(calibrator.isRunning());
16457         assertNull(calibrator.getEstimatedBiases());
16458         assertFalse(calibrator.getEstimatedBiases(null));
16459         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16460         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16461         assertNull(calibrator.getEstimatedBiasFx());
16462         assertNull(calibrator.getEstimatedBiasFy());
16463         assertNull(calibrator.getEstimatedBiasFz());
16464         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16465         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16466         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16467         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16468         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16469         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16470         assertNull(calibrator.getEstimatedMa());
16471         assertNull(calibrator.getEstimatedSx());
16472         assertNull(calibrator.getEstimatedSy());
16473         assertNull(calibrator.getEstimatedSz());
16474         assertNull(calibrator.getEstimatedMxy());
16475         assertNull(calibrator.getEstimatedMxz());
16476         assertNull(calibrator.getEstimatedMyx());
16477         assertNull(calibrator.getEstimatedMyz());
16478         assertNull(calibrator.getEstimatedMzx());
16479         assertNull(calibrator.getEstimatedMzy());
16480         assertNull(calibrator.getEstimatedCovariance());
16481         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16482 
16483         // Force IllegalArgumentException
16484         calibrator = null;
16485         try {
16486             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16487                     measurements, true, new double[1]);
16488             fail("IllegalArgumentException expected but not thrown");
16489         } catch (final IllegalArgumentException ignore) {
16490         }
16491         assertNull(calibrator);
16492     }
16493 
16494     @Test
16495     public void testConstructor140() throws WrongSizeException {
16496         final Collection<StandardDeviationBodyKinematics> measurements =
16497                 Collections.emptyList();
16498 
16499         final Matrix ba = generateBa();
16500         final double[] bias = ba.getBuffer();
16501         final double biasX = ba.getElementAtIndex(0);
16502         final double biasY = ba.getElementAtIndex(1);
16503         final double biasZ = ba.getElementAtIndex(2);
16504 
16505         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16506         final double latitude = Math.toRadians(
16507                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16508         final double longitude = Math.toRadians(
16509                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16510         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16511         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16512         final NEDVelocity nedVelocity = new NEDVelocity();
16513         final ECEFPosition ecefPosition = new ECEFPosition();
16514         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16515         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16516                 ecefPosition, ecefVelocity);
16517 
16518         KnownPositionAccelerometerCalibrator calibrator =
16519                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16520                         true, bias, this);
16521 
16522         // check default values
16523         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16524         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16525         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16526         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16527         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16528         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16529         final Acceleration bx2 = new Acceleration(0.0,
16530                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16531         calibrator.getInitialBiasXAsAcceleration(bx2);
16532         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16533         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16534         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16535         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16536         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16537         final Acceleration by2 = new Acceleration(0.0,
16538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16539         calibrator.getInitialBiasYAsAcceleration(by2);
16540         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16541         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16542         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16543         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16544         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16545         final Acceleration bz2 = new Acceleration(0.0,
16546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16547         calibrator.getInitialBiasZAsAcceleration(bz2);
16548         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16549         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16550         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16551         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16552         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16553         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16554         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16555         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16556         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16557         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16558         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16559         final double[] bias1 = calibrator.getInitialBias();
16560         assertArrayEquals(bias1, bias, 0.0);
16561         final double[] bias2 = new double[3];
16562         calibrator.getInitialBias(bias2);
16563         assertArrayEquals(bias1, bias2, 0.0);
16564         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16565         assertEquals(b1, ba);
16566         final Matrix b2 = new Matrix(3, 1);
16567         calibrator.getInitialBiasAsMatrix(b2);
16568         assertEquals(b1, b2);
16569         final Matrix ma1 = calibrator.getInitialMa();
16570         assertEquals(ma1, new Matrix(3, 3));
16571         final Matrix ma2 = new Matrix(3, 3);
16572         calibrator.getInitialMa(ma2);
16573         assertEquals(ma1, ma2);
16574         assertSame(calibrator.getEcefPosition(), ecefPosition);
16575         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16576         final NEDPosition nedPosition1 = new NEDPosition();
16577         assertTrue(calibrator.getNedPosition(nedPosition1));
16578         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16579         assertSame(calibrator.getMeasurements(), measurements);
16580         assertTrue(calibrator.isCommonAxisUsed());
16581         assertSame(calibrator.getListener(), this);
16582         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16583         assertFalse(calibrator.isReady());
16584         assertFalse(calibrator.isRunning());
16585         assertNull(calibrator.getEstimatedBiases());
16586         assertFalse(calibrator.getEstimatedBiases(null));
16587         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16588         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16589         assertNull(calibrator.getEstimatedBiasFx());
16590         assertNull(calibrator.getEstimatedBiasFy());
16591         assertNull(calibrator.getEstimatedBiasFz());
16592         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16593         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16594         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16595         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16596         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16597         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16598         assertNull(calibrator.getEstimatedMa());
16599         assertNull(calibrator.getEstimatedSx());
16600         assertNull(calibrator.getEstimatedSy());
16601         assertNull(calibrator.getEstimatedSz());
16602         assertNull(calibrator.getEstimatedMxy());
16603         assertNull(calibrator.getEstimatedMxz());
16604         assertNull(calibrator.getEstimatedMyx());
16605         assertNull(calibrator.getEstimatedMyz());
16606         assertNull(calibrator.getEstimatedMzx());
16607         assertNull(calibrator.getEstimatedMzy());
16608         assertNull(calibrator.getEstimatedCovariance());
16609         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16610 
16611         // Force IllegalArgumentException
16612         calibrator = null;
16613         try {
16614             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16615                     measurements, true, new double[1], this);
16616             fail("IllegalArgumentException expected but not thrown");
16617         } catch (final IllegalArgumentException ignore) {
16618         }
16619         assertNull(calibrator);
16620     }
16621 
16622     @Test
16623     public void testConstructor141() throws WrongSizeException {
16624         final Matrix ba = generateBa();
16625         final double[] bias = ba.getBuffer();
16626         final double biasX = ba.getElementAtIndex(0);
16627         final double biasY = ba.getElementAtIndex(1);
16628         final double biasZ = ba.getElementAtIndex(2);
16629 
16630         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16631         final double latitude = Math.toRadians(
16632                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16633         final double longitude = Math.toRadians(
16634                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16635         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16636         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16637         final NEDVelocity nedVelocity = new NEDVelocity();
16638         final ECEFPosition ecefPosition = new ECEFPosition();
16639         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16640         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16641                 ecefPosition, ecefVelocity);
16642 
16643         KnownPositionAccelerometerCalibrator calibrator =
16644                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
16645 
16646         // check default values
16647         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16648         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16649         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16650         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16651         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16652         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16653         final Acceleration bx2 = new Acceleration(0.0,
16654                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16655         calibrator.getInitialBiasXAsAcceleration(bx2);
16656         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16657         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16658         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16659         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16660         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16661         final Acceleration by2 = new Acceleration(0.0,
16662                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16663         calibrator.getInitialBiasYAsAcceleration(by2);
16664         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16665         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16666         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16667         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16668         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16669         final Acceleration bz2 = new Acceleration(0.0,
16670                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16671         calibrator.getInitialBiasZAsAcceleration(bz2);
16672         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16673         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16674         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16675         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16676         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16677         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16678         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16679         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16680         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16681         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16682         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16683         final double[] bias1 = calibrator.getInitialBias();
16684         assertArrayEquals(bias1, bias, 0.0);
16685         final double[] bias2 = new double[3];
16686         calibrator.getInitialBias(bias2);
16687         assertArrayEquals(bias1, bias2, 0.0);
16688         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16689         assertEquals(b1, ba);
16690         final Matrix b2 = new Matrix(3, 1);
16691         calibrator.getInitialBiasAsMatrix(b2);
16692         assertEquals(b1, b2);
16693         final Matrix ma1 = calibrator.getInitialMa();
16694         assertEquals(ma1, new Matrix(3, 3));
16695         final Matrix ma2 = new Matrix(3, 3);
16696         calibrator.getInitialMa(ma2);
16697         assertEquals(ma1, ma2);
16698         assertSame(calibrator.getEcefPosition(), ecefPosition);
16699         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16700         final NEDPosition nedPosition1 = new NEDPosition();
16701         assertTrue(calibrator.getNedPosition(nedPosition1));
16702         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16703         assertNull(calibrator.getMeasurements());
16704         assertFalse(calibrator.isCommonAxisUsed());
16705         assertNull(calibrator.getListener());
16706         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16707         assertFalse(calibrator.isReady());
16708         assertFalse(calibrator.isRunning());
16709         assertNull(calibrator.getEstimatedBiases());
16710         assertFalse(calibrator.getEstimatedBiases(null));
16711         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16712         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16713         assertNull(calibrator.getEstimatedBiasFx());
16714         assertNull(calibrator.getEstimatedBiasFy());
16715         assertNull(calibrator.getEstimatedBiasFz());
16716         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16717         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16718         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16719         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16720         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16721         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16722         assertNull(calibrator.getEstimatedMa());
16723         assertNull(calibrator.getEstimatedSx());
16724         assertNull(calibrator.getEstimatedSy());
16725         assertNull(calibrator.getEstimatedSz());
16726         assertNull(calibrator.getEstimatedMxy());
16727         assertNull(calibrator.getEstimatedMxz());
16728         assertNull(calibrator.getEstimatedMyx());
16729         assertNull(calibrator.getEstimatedMyz());
16730         assertNull(calibrator.getEstimatedMzx());
16731         assertNull(calibrator.getEstimatedMzy());
16732         assertNull(calibrator.getEstimatedCovariance());
16733         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16734 
16735         // Force IllegalArgumentException
16736         calibrator = null;
16737         try {
16738             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16739                     new Matrix(1, 1));
16740             fail("IllegalArgumentException expected but not thrown");
16741         } catch (final IllegalArgumentException ignore) {
16742         }
16743         try {
16744             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16745                     new Matrix(1, 3));
16746             fail("IllegalArgumentException expected but not thrown");
16747         } catch (final IllegalArgumentException ignore) {
16748         }
16749         assertNull(calibrator);
16750     }
16751 
16752     @Test
16753     public void testConstructor142() throws WrongSizeException {
16754         final Matrix ba = generateBa();
16755         final double[] bias = ba.getBuffer();
16756         final double biasX = ba.getElementAtIndex(0);
16757         final double biasY = ba.getElementAtIndex(1);
16758         final double biasZ = ba.getElementAtIndex(2);
16759 
16760         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16761         final double latitude = Math.toRadians(
16762                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16763         final double longitude = Math.toRadians(
16764                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16765         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16766         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16767         final NEDVelocity nedVelocity = new NEDVelocity();
16768         final ECEFPosition ecefPosition = new ECEFPosition();
16769         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16770         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16771                 ecefPosition, ecefVelocity);
16772 
16773         KnownPositionAccelerometerCalibrator calibrator =
16774                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
16775                         this);
16776 
16777         // check default values
16778         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16779         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16780         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16781         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16782         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16783         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16784         final Acceleration bx2 = new Acceleration(0.0,
16785                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16786         calibrator.getInitialBiasXAsAcceleration(bx2);
16787         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16788         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16789         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16790         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16791         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16792         final Acceleration by2 = new Acceleration(0.0,
16793                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16794         calibrator.getInitialBiasYAsAcceleration(by2);
16795         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16796         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16797         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16798         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16799         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16800         final Acceleration bz2 = new Acceleration(0.0,
16801                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16802         calibrator.getInitialBiasZAsAcceleration(bz2);
16803         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16804         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16805         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16806         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16807         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16808         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16809         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16810         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16811         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16812         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16813         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16814         final double[] bias1 = calibrator.getInitialBias();
16815         assertArrayEquals(bias1, bias, 0.0);
16816         final double[] bias2 = new double[3];
16817         calibrator.getInitialBias(bias2);
16818         assertArrayEquals(bias1, bias2, 0.0);
16819         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16820         assertEquals(b1, ba);
16821         final Matrix b2 = new Matrix(3, 1);
16822         calibrator.getInitialBiasAsMatrix(b2);
16823         assertEquals(b1, b2);
16824         final Matrix ma1 = calibrator.getInitialMa();
16825         assertEquals(ma1, new Matrix(3, 3));
16826         final Matrix ma2 = new Matrix(3, 3);
16827         calibrator.getInitialMa(ma2);
16828         assertEquals(ma1, ma2);
16829         assertSame(calibrator.getEcefPosition(), ecefPosition);
16830         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16831         final NEDPosition nedPosition1 = new NEDPosition();
16832         assertTrue(calibrator.getNedPosition(nedPosition1));
16833         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16834         assertNull(calibrator.getMeasurements());
16835         assertFalse(calibrator.isCommonAxisUsed());
16836         assertSame(calibrator.getListener(), this);
16837         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16838         assertFalse(calibrator.isReady());
16839         assertFalse(calibrator.isRunning());
16840         assertNull(calibrator.getEstimatedBiases());
16841         assertFalse(calibrator.getEstimatedBiases(null));
16842         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16843         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16844         assertNull(calibrator.getEstimatedBiasFx());
16845         assertNull(calibrator.getEstimatedBiasFy());
16846         assertNull(calibrator.getEstimatedBiasFz());
16847         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16848         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16849         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16850         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16851         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16852         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16853         assertNull(calibrator.getEstimatedMa());
16854         assertNull(calibrator.getEstimatedSx());
16855         assertNull(calibrator.getEstimatedSy());
16856         assertNull(calibrator.getEstimatedSz());
16857         assertNull(calibrator.getEstimatedMxy());
16858         assertNull(calibrator.getEstimatedMxz());
16859         assertNull(calibrator.getEstimatedMyx());
16860         assertNull(calibrator.getEstimatedMyz());
16861         assertNull(calibrator.getEstimatedMzx());
16862         assertNull(calibrator.getEstimatedMzy());
16863         assertNull(calibrator.getEstimatedCovariance());
16864         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16865 
16866         // Force IllegalArgumentException
16867         calibrator = null;
16868         try {
16869             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16870                     new Matrix(1, 1), this);
16871             fail("IllegalArgumentException expected but not thrown");
16872         } catch (final IllegalArgumentException ignore) {
16873         }
16874         try {
16875             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16876                     new Matrix(1, 3), this);
16877             fail("IllegalArgumentException expected but not thrown");
16878         } catch (final IllegalArgumentException ignore) {
16879         }
16880         assertNull(calibrator);
16881     }
16882 
16883     @Test
16884     public void testConstructor143() throws WrongSizeException {
16885         final Collection<StandardDeviationBodyKinematics> measurements =
16886                 Collections.emptyList();
16887 
16888         final Matrix ba = generateBa();
16889         final double[] bias = ba.getBuffer();
16890         final double biasX = ba.getElementAtIndex(0);
16891         final double biasY = ba.getElementAtIndex(1);
16892         final double biasZ = ba.getElementAtIndex(2);
16893 
16894         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16895         final double latitude = Math.toRadians(
16896                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16897         final double longitude = Math.toRadians(
16898                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16899         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16900         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16901         final NEDVelocity nedVelocity = new NEDVelocity();
16902         final ECEFPosition ecefPosition = new ECEFPosition();
16903         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16904         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16905                 ecefPosition, ecefVelocity);
16906 
16907         KnownPositionAccelerometerCalibrator calibrator =
16908                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16909                         ba);
16910 
16911         // check default values
16912         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16913         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16914         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16915         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16916         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16917         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16918         final Acceleration bx2 = new Acceleration(0.0,
16919                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16920         calibrator.getInitialBiasXAsAcceleration(bx2);
16921         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16922         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16923         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16924         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16925         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16926         final Acceleration by2 = new Acceleration(0.0,
16927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16928         calibrator.getInitialBiasYAsAcceleration(by2);
16929         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16930         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16931         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16932         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16933         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16934         final Acceleration bz2 = new Acceleration(0.0,
16935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16936         calibrator.getInitialBiasZAsAcceleration(bz2);
16937         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16938         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16939         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16940         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16941         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16942         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16943         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16944         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16945         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16946         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16947         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16948         final double[] bias1 = calibrator.getInitialBias();
16949         assertArrayEquals(bias1, bias, 0.0);
16950         final double[] bias2 = new double[3];
16951         calibrator.getInitialBias(bias2);
16952         assertArrayEquals(bias1, bias2, 0.0);
16953         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16954         assertEquals(b1, ba);
16955         final Matrix b2 = new Matrix(3, 1);
16956         calibrator.getInitialBiasAsMatrix(b2);
16957         assertEquals(b1, b2);
16958         final Matrix ma1 = calibrator.getInitialMa();
16959         assertEquals(ma1, new Matrix(3, 3));
16960         final Matrix ma2 = new Matrix(3, 3);
16961         calibrator.getInitialMa(ma2);
16962         assertEquals(ma1, ma2);
16963         assertSame(calibrator.getEcefPosition(), ecefPosition);
16964         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16965         final NEDPosition nedPosition1 = new NEDPosition();
16966         assertTrue(calibrator.getNedPosition(nedPosition1));
16967         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16968         assertSame(calibrator.getMeasurements(), measurements);
16969         assertFalse(calibrator.isCommonAxisUsed());
16970         assertNull(calibrator.getListener());
16971         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16972         assertFalse(calibrator.isReady());
16973         assertFalse(calibrator.isRunning());
16974         assertNull(calibrator.getEstimatedBiases());
16975         assertFalse(calibrator.getEstimatedBiases(null));
16976         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16977         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16978         assertNull(calibrator.getEstimatedBiasFx());
16979         assertNull(calibrator.getEstimatedBiasFy());
16980         assertNull(calibrator.getEstimatedBiasFz());
16981         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16982         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16983         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16984         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16985         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16986         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16987         assertNull(calibrator.getEstimatedMa());
16988         assertNull(calibrator.getEstimatedSx());
16989         assertNull(calibrator.getEstimatedSy());
16990         assertNull(calibrator.getEstimatedSz());
16991         assertNull(calibrator.getEstimatedMxy());
16992         assertNull(calibrator.getEstimatedMxz());
16993         assertNull(calibrator.getEstimatedMyx());
16994         assertNull(calibrator.getEstimatedMyz());
16995         assertNull(calibrator.getEstimatedMzx());
16996         assertNull(calibrator.getEstimatedMzy());
16997         assertNull(calibrator.getEstimatedCovariance());
16998         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16999 
17000         // Force IllegalArgumentException
17001         calibrator = null;
17002         try {
17003             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17004                     measurements, new Matrix(1, 1));
17005             fail("IllegalArgumentException expected but not thrown");
17006         } catch (final IllegalArgumentException ignore) {
17007         }
17008         try {
17009             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17010                     measurements, new Matrix(1, 3));
17011             fail("IllegalArgumentException expected but not thrown");
17012         } catch (final IllegalArgumentException ignore) {
17013         }
17014         assertNull(calibrator);
17015     }
17016 
17017     @Test
17018     public void testConstructor144() throws WrongSizeException {
17019         final Collection<StandardDeviationBodyKinematics> measurements =
17020                 Collections.emptyList();
17021 
17022         final Matrix ba = generateBa();
17023         final double[] bias = ba.getBuffer();
17024         final double biasX = ba.getElementAtIndex(0);
17025         final double biasY = ba.getElementAtIndex(1);
17026         final double biasZ = ba.getElementAtIndex(2);
17027 
17028         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17029         final double latitude = Math.toRadians(
17030                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17031         final double longitude = Math.toRadians(
17032                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17033         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17034         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17035         final NEDVelocity nedVelocity = new NEDVelocity();
17036         final ECEFPosition ecefPosition = new ECEFPosition();
17037         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17038         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17039                 ecefPosition, ecefVelocity);
17040 
17041         KnownPositionAccelerometerCalibrator calibrator =
17042                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17043                         ba, this);
17044 
17045         // check default values
17046         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17047         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17048         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17049         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17050         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17051         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17052         final Acceleration bx2 = new Acceleration(0.0,
17053                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17054         calibrator.getInitialBiasXAsAcceleration(bx2);
17055         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17056         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17057         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17058         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17059         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17060         final Acceleration by2 = new Acceleration(0.0,
17061                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17062         calibrator.getInitialBiasYAsAcceleration(by2);
17063         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17064         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17065         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17066         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17067         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17068         final Acceleration bz2 = new Acceleration(0.0,
17069                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17070         calibrator.getInitialBiasZAsAcceleration(bz2);
17071         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17072         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17073         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17074         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17075         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17076         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17077         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17078         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17079         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17080         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17081         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17082         final double[] bias1 = calibrator.getInitialBias();
17083         assertArrayEquals(bias1, bias, 0.0);
17084         final double[] bias2 = new double[3];
17085         calibrator.getInitialBias(bias2);
17086         assertArrayEquals(bias1, bias2, 0.0);
17087         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17088         assertEquals(b1, ba);
17089         final Matrix b2 = new Matrix(3, 1);
17090         calibrator.getInitialBiasAsMatrix(b2);
17091         assertEquals(b1, b2);
17092         final Matrix ma1 = calibrator.getInitialMa();
17093         assertEquals(ma1, new Matrix(3, 3));
17094         final Matrix ma2 = new Matrix(3, 3);
17095         calibrator.getInitialMa(ma2);
17096         assertEquals(ma1, ma2);
17097         assertSame(calibrator.getEcefPosition(), ecefPosition);
17098         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17099         final NEDPosition nedPosition1 = new NEDPosition();
17100         assertTrue(calibrator.getNedPosition(nedPosition1));
17101         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17102         assertSame(calibrator.getMeasurements(), measurements);
17103         assertFalse(calibrator.isCommonAxisUsed());
17104         assertSame(calibrator.getListener(), this);
17105         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17106         assertFalse(calibrator.isReady());
17107         assertFalse(calibrator.isRunning());
17108         assertNull(calibrator.getEstimatedBiases());
17109         assertFalse(calibrator.getEstimatedBiases(null));
17110         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17111         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17112         assertNull(calibrator.getEstimatedBiasFx());
17113         assertNull(calibrator.getEstimatedBiasFy());
17114         assertNull(calibrator.getEstimatedBiasFz());
17115         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17116         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17117         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17118         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17119         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17120         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17121         assertNull(calibrator.getEstimatedMa());
17122         assertNull(calibrator.getEstimatedSx());
17123         assertNull(calibrator.getEstimatedSy());
17124         assertNull(calibrator.getEstimatedSz());
17125         assertNull(calibrator.getEstimatedMxy());
17126         assertNull(calibrator.getEstimatedMxz());
17127         assertNull(calibrator.getEstimatedMyx());
17128         assertNull(calibrator.getEstimatedMyz());
17129         assertNull(calibrator.getEstimatedMzx());
17130         assertNull(calibrator.getEstimatedMzy());
17131         assertNull(calibrator.getEstimatedCovariance());
17132         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17133 
17134         // Force IllegalArgumentException
17135         calibrator = null;
17136         try {
17137             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17138                     measurements, new Matrix(1, 1), this);
17139             fail("IllegalArgumentException expected but not thrown");
17140         } catch (final IllegalArgumentException ignore) {
17141         }
17142         try {
17143             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17144                     measurements, new Matrix(1, 3), this);
17145             fail("IllegalArgumentException expected but not thrown");
17146         } catch (final IllegalArgumentException ignore) {
17147         }
17148         assertNull(calibrator);
17149     }
17150 
17151     @Test
17152     public void testConstructor145() throws WrongSizeException {
17153         final Matrix ba = generateBa();
17154         final double[] bias = ba.getBuffer();
17155         final double biasX = ba.getElementAtIndex(0);
17156         final double biasY = ba.getElementAtIndex(1);
17157         final double biasZ = ba.getElementAtIndex(2);
17158 
17159         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17160         final double latitude = Math.toRadians(
17161                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17162         final double longitude = Math.toRadians(
17163                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17164         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17165         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17166         final NEDVelocity nedVelocity = new NEDVelocity();
17167         final ECEFPosition ecefPosition = new ECEFPosition();
17168         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17169         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17170                 ecefPosition, ecefVelocity);
17171 
17172         KnownPositionAccelerometerCalibrator calibrator =
17173                 new KnownPositionAccelerometerCalibrator(ecefPosition,
17174                         true, ba);
17175 
17176         // check default values
17177         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17178         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17179         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17180         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17181         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17182         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17183         final Acceleration bx2 = new Acceleration(0.0,
17184                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17185         calibrator.getInitialBiasXAsAcceleration(bx2);
17186         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17187         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17188         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17189         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17190         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17191         final Acceleration by2 = new Acceleration(0.0,
17192                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17193         calibrator.getInitialBiasYAsAcceleration(by2);
17194         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17195         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17196         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17197         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17198         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17199         final Acceleration bz2 = new Acceleration(0.0,
17200                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17201         calibrator.getInitialBiasZAsAcceleration(bz2);
17202         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17203         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17204         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17205         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17206         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17207         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17208         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17209         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17210         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17211         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17212         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17213         final double[] bias1 = calibrator.getInitialBias();
17214         assertArrayEquals(bias1, bias, 0.0);
17215         final double[] bias2 = new double[3];
17216         calibrator.getInitialBias(bias2);
17217         assertArrayEquals(bias1, bias2, 0.0);
17218         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17219         assertEquals(b1, ba);
17220         final Matrix b2 = new Matrix(3, 1);
17221         calibrator.getInitialBiasAsMatrix(b2);
17222         assertEquals(b1, b2);
17223         final Matrix ma1 = calibrator.getInitialMa();
17224         assertEquals(ma1, new Matrix(3, 3));
17225         final Matrix ma2 = new Matrix(3, 3);
17226         calibrator.getInitialMa(ma2);
17227         assertEquals(ma1, ma2);
17228         assertSame(calibrator.getEcefPosition(), ecefPosition);
17229         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17230         final NEDPosition nedPosition1 = new NEDPosition();
17231         assertTrue(calibrator.getNedPosition(nedPosition1));
17232         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17233         assertNull(calibrator.getMeasurements());
17234         assertTrue(calibrator.isCommonAxisUsed());
17235         assertNull(calibrator.getListener());
17236         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17237         assertFalse(calibrator.isReady());
17238         assertFalse(calibrator.isRunning());
17239         assertNull(calibrator.getEstimatedBiases());
17240         assertFalse(calibrator.getEstimatedBiases(null));
17241         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17242         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17243         assertNull(calibrator.getEstimatedBiasFx());
17244         assertNull(calibrator.getEstimatedBiasFy());
17245         assertNull(calibrator.getEstimatedBiasFz());
17246         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17247         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17248         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17249         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17250         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17251         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17252         assertNull(calibrator.getEstimatedMa());
17253         assertNull(calibrator.getEstimatedSx());
17254         assertNull(calibrator.getEstimatedSy());
17255         assertNull(calibrator.getEstimatedSz());
17256         assertNull(calibrator.getEstimatedMxy());
17257         assertNull(calibrator.getEstimatedMxz());
17258         assertNull(calibrator.getEstimatedMyx());
17259         assertNull(calibrator.getEstimatedMyz());
17260         assertNull(calibrator.getEstimatedMzx());
17261         assertNull(calibrator.getEstimatedMzy());
17262         assertNull(calibrator.getEstimatedCovariance());
17263         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17264 
17265         // Force IllegalArgumentException
17266         calibrator = null;
17267         try {
17268             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17269                     true, new Matrix(1, 1));
17270             fail("IllegalArgumentException expected but not thrown");
17271         } catch (final IllegalArgumentException ignore) {
17272         }
17273         try {
17274             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17275                     true, new Matrix(1, 3));
17276             fail("IllegalArgumentException expected but not thrown");
17277         } catch (final IllegalArgumentException ignore) {
17278         }
17279         assertNull(calibrator);
17280     }
17281 
17282     @Test
17283     public void testConstructor146() throws WrongSizeException {
17284         final Matrix ba = generateBa();
17285         final double[] bias = ba.getBuffer();
17286         final double biasX = ba.getElementAtIndex(0);
17287         final double biasY = ba.getElementAtIndex(1);
17288         final double biasZ = ba.getElementAtIndex(2);
17289 
17290         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17291         final double latitude = Math.toRadians(
17292                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17293         final double longitude = Math.toRadians(
17294                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17295         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17296         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17297         final NEDVelocity nedVelocity = new NEDVelocity();
17298         final ECEFPosition ecefPosition = new ECEFPosition();
17299         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17300         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17301                 ecefPosition, ecefVelocity);
17302 
17303         KnownPositionAccelerometerCalibrator calibrator =
17304                 new KnownPositionAccelerometerCalibrator(ecefPosition,
17305                         true, ba, this);
17306 
17307         // check default values
17308         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17309         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17310         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17311         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17312         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17313         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17314         final Acceleration bx2 = new Acceleration(0.0,
17315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17316         calibrator.getInitialBiasXAsAcceleration(bx2);
17317         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17318         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17319         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17320         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17321         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17322         final Acceleration by2 = new Acceleration(0.0,
17323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17324         calibrator.getInitialBiasYAsAcceleration(by2);
17325         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17326         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17327         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17328         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17329         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17330         final Acceleration bz2 = new Acceleration(0.0,
17331                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17332         calibrator.getInitialBiasZAsAcceleration(bz2);
17333         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17334         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17335         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17336         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17337         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17338         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17339         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17340         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17341         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17342         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17343         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17344         final double[] bias1 = calibrator.getInitialBias();
17345         assertArrayEquals(bias1, bias, 0.0);
17346         final double[] bias2 = new double[3];
17347         calibrator.getInitialBias(bias2);
17348         assertArrayEquals(bias1, bias2, 0.0);
17349         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17350         assertEquals(b1, ba);
17351         final Matrix b2 = new Matrix(3, 1);
17352         calibrator.getInitialBiasAsMatrix(b2);
17353         assertEquals(b1, b2);
17354         final Matrix ma1 = calibrator.getInitialMa();
17355         assertEquals(ma1, new Matrix(3, 3));
17356         final Matrix ma2 = new Matrix(3, 3);
17357         calibrator.getInitialMa(ma2);
17358         assertEquals(ma1, ma2);
17359         assertSame(calibrator.getEcefPosition(), ecefPosition);
17360         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17361         final NEDPosition nedPosition1 = new NEDPosition();
17362         assertTrue(calibrator.getNedPosition(nedPosition1));
17363         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17364         assertNull(calibrator.getMeasurements());
17365         assertTrue(calibrator.isCommonAxisUsed());
17366         assertSame(calibrator.getListener(), this);
17367         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17368         assertFalse(calibrator.isReady());
17369         assertFalse(calibrator.isRunning());
17370         assertNull(calibrator.getEstimatedBiases());
17371         assertFalse(calibrator.getEstimatedBiases(null));
17372         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17373         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17374         assertNull(calibrator.getEstimatedBiasFx());
17375         assertNull(calibrator.getEstimatedBiasFy());
17376         assertNull(calibrator.getEstimatedBiasFz());
17377         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17378         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17379         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17380         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17381         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17382         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17383         assertNull(calibrator.getEstimatedMa());
17384         assertNull(calibrator.getEstimatedSx());
17385         assertNull(calibrator.getEstimatedSy());
17386         assertNull(calibrator.getEstimatedSz());
17387         assertNull(calibrator.getEstimatedMxy());
17388         assertNull(calibrator.getEstimatedMxz());
17389         assertNull(calibrator.getEstimatedMyx());
17390         assertNull(calibrator.getEstimatedMyz());
17391         assertNull(calibrator.getEstimatedMzx());
17392         assertNull(calibrator.getEstimatedMzy());
17393         assertNull(calibrator.getEstimatedCovariance());
17394         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17395 
17396         // Force IllegalArgumentException
17397         calibrator = null;
17398         try {
17399             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17400                     true, new Matrix(1, 1),
17401                     this);
17402             fail("IllegalArgumentException expected but not thrown");
17403         } catch (final IllegalArgumentException ignore) {
17404         }
17405         try {
17406             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17407                     true, new Matrix(1, 3),
17408                     this);
17409             fail("IllegalArgumentException expected but not thrown");
17410         } catch (final IllegalArgumentException ignore) {
17411         }
17412         assertNull(calibrator);
17413     }
17414 
17415     @Test
17416     public void testConstructor147() throws WrongSizeException {
17417         final Collection<StandardDeviationBodyKinematics> measurements =
17418                 Collections.emptyList();
17419 
17420         final Matrix ba = generateBa();
17421         final double[] bias = ba.getBuffer();
17422         final double biasX = ba.getElementAtIndex(0);
17423         final double biasY = ba.getElementAtIndex(1);
17424         final double biasZ = ba.getElementAtIndex(2);
17425 
17426         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17427         final double latitude = Math.toRadians(
17428                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17429         final double longitude = Math.toRadians(
17430                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17431         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17432         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17433         final NEDVelocity nedVelocity = new NEDVelocity();
17434         final ECEFPosition ecefPosition = new ECEFPosition();
17435         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17436         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17437                 ecefPosition, ecefVelocity);
17438 
17439         KnownPositionAccelerometerCalibrator calibrator =
17440                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17441                         true, ba);
17442 
17443         // check default values
17444         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17445         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17446         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17447         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17448         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17449         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17450         final Acceleration bx2 = new Acceleration(0.0,
17451                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17452         calibrator.getInitialBiasXAsAcceleration(bx2);
17453         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17454         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17455         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17456         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17457         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17458         final Acceleration by2 = new Acceleration(0.0,
17459                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17460         calibrator.getInitialBiasYAsAcceleration(by2);
17461         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17462         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17463         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17464         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17465         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17466         final Acceleration bz2 = new Acceleration(0.0,
17467                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17468         calibrator.getInitialBiasZAsAcceleration(bz2);
17469         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17470         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17471         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17472         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17473         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17474         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17475         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17476         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17477         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17478         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17479         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17480         final double[] bias1 = calibrator.getInitialBias();
17481         assertArrayEquals(bias1, bias, 0.0);
17482         final double[] bias2 = new double[3];
17483         calibrator.getInitialBias(bias2);
17484         assertArrayEquals(bias1, bias2, 0.0);
17485         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17486         assertEquals(b1, ba);
17487         final Matrix b2 = new Matrix(3, 1);
17488         calibrator.getInitialBiasAsMatrix(b2);
17489         assertEquals(b1, b2);
17490         final Matrix ma1 = calibrator.getInitialMa();
17491         assertEquals(ma1, new Matrix(3, 3));
17492         final Matrix ma2 = new Matrix(3, 3);
17493         calibrator.getInitialMa(ma2);
17494         assertEquals(ma1, ma2);
17495         assertSame(calibrator.getEcefPosition(), ecefPosition);
17496         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17497         final NEDPosition nedPosition1 = new NEDPosition();
17498         assertTrue(calibrator.getNedPosition(nedPosition1));
17499         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17500         assertSame(calibrator.getMeasurements(), measurements);
17501         assertTrue(calibrator.isCommonAxisUsed());
17502         assertNull(calibrator.getListener());
17503         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17504         assertFalse(calibrator.isReady());
17505         assertFalse(calibrator.isRunning());
17506         assertNull(calibrator.getEstimatedBiases());
17507         assertFalse(calibrator.getEstimatedBiases(null));
17508         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17509         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17510         assertNull(calibrator.getEstimatedBiasFx());
17511         assertNull(calibrator.getEstimatedBiasFy());
17512         assertNull(calibrator.getEstimatedBiasFz());
17513         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17514         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17515         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17516         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17517         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17518         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17519         assertNull(calibrator.getEstimatedMa());
17520         assertNull(calibrator.getEstimatedSx());
17521         assertNull(calibrator.getEstimatedSy());
17522         assertNull(calibrator.getEstimatedSz());
17523         assertNull(calibrator.getEstimatedMxy());
17524         assertNull(calibrator.getEstimatedMxz());
17525         assertNull(calibrator.getEstimatedMyx());
17526         assertNull(calibrator.getEstimatedMyz());
17527         assertNull(calibrator.getEstimatedMzx());
17528         assertNull(calibrator.getEstimatedMzy());
17529         assertNull(calibrator.getEstimatedCovariance());
17530         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17531 
17532         // Force IllegalArgumentException
17533         calibrator = null;
17534         try {
17535             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17536                     measurements, true,
17537                     new Matrix(1, 1));
17538             fail("IllegalArgumentException expected but not thrown");
17539         } catch (final IllegalArgumentException ignore) {
17540         }
17541         try {
17542             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17543                     measurements,true,
17544                     new Matrix(1, 3));
17545             fail("IllegalArgumentException expected but not thrown");
17546         } catch (final IllegalArgumentException ignore) {
17547         }
17548         assertNull(calibrator);
17549     }
17550 
17551     @Test
17552     public void testConstructor148() throws WrongSizeException {
17553         final Collection<StandardDeviationBodyKinematics> measurements =
17554                 Collections.emptyList();
17555 
17556         final Matrix ba = generateBa();
17557         final double[] bias = ba.getBuffer();
17558         final double biasX = ba.getElementAtIndex(0);
17559         final double biasY = ba.getElementAtIndex(1);
17560         final double biasZ = ba.getElementAtIndex(2);
17561 
17562         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17563         final double latitude = Math.toRadians(
17564                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17565         final double longitude = Math.toRadians(
17566                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17567         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17568         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17569         final NEDVelocity nedVelocity = new NEDVelocity();
17570         final ECEFPosition ecefPosition = new ECEFPosition();
17571         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17572         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17573                 ecefPosition, ecefVelocity);
17574 
17575         KnownPositionAccelerometerCalibrator calibrator =
17576                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17577                         true, ba, this);
17578 
17579         // check default values
17580         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17581         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17582         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17583         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17584         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17585         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17586         final Acceleration bx2 = new Acceleration(0.0,
17587                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17588         calibrator.getInitialBiasXAsAcceleration(bx2);
17589         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17590         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17591         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17592         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17593         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17594         final Acceleration by2 = new Acceleration(0.0,
17595                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17596         calibrator.getInitialBiasYAsAcceleration(by2);
17597         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17598         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17599         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17600         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17601         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17602         final Acceleration bz2 = new Acceleration(0.0,
17603                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17604         calibrator.getInitialBiasZAsAcceleration(bz2);
17605         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17606         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17607         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17608         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17609         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17610         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17611         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17612         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17613         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17614         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17615         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17616         final double[] bias1 = calibrator.getInitialBias();
17617         assertArrayEquals(bias1, bias, 0.0);
17618         final double[] bias2 = new double[3];
17619         calibrator.getInitialBias(bias2);
17620         assertArrayEquals(bias1, bias2, 0.0);
17621         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17622         assertEquals(b1, ba);
17623         final Matrix b2 = new Matrix(3, 1);
17624         calibrator.getInitialBiasAsMatrix(b2);
17625         assertEquals(b1, b2);
17626         final Matrix ma1 = calibrator.getInitialMa();
17627         assertEquals(ma1, new Matrix(3, 3));
17628         final Matrix ma2 = new Matrix(3, 3);
17629         calibrator.getInitialMa(ma2);
17630         assertEquals(ma1, ma2);
17631         assertSame(calibrator.getEcefPosition(), ecefPosition);
17632         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17633         final NEDPosition nedPosition1 = new NEDPosition();
17634         assertTrue(calibrator.getNedPosition(nedPosition1));
17635         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17636         assertSame(calibrator.getMeasurements(), measurements);
17637         assertTrue(calibrator.isCommonAxisUsed());
17638         assertSame(calibrator.getListener(), this);
17639         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17640         assertFalse(calibrator.isReady());
17641         assertFalse(calibrator.isRunning());
17642         assertNull(calibrator.getEstimatedBiases());
17643         assertFalse(calibrator.getEstimatedBiases(null));
17644         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17645         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17646         assertNull(calibrator.getEstimatedBiasFx());
17647         assertNull(calibrator.getEstimatedBiasFy());
17648         assertNull(calibrator.getEstimatedBiasFz());
17649         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17650         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17651         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17652         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17653         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17654         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17655         assertNull(calibrator.getEstimatedMa());
17656         assertNull(calibrator.getEstimatedSx());
17657         assertNull(calibrator.getEstimatedSy());
17658         assertNull(calibrator.getEstimatedSz());
17659         assertNull(calibrator.getEstimatedMxy());
17660         assertNull(calibrator.getEstimatedMxz());
17661         assertNull(calibrator.getEstimatedMyx());
17662         assertNull(calibrator.getEstimatedMyz());
17663         assertNull(calibrator.getEstimatedMzx());
17664         assertNull(calibrator.getEstimatedMzy());
17665         assertNull(calibrator.getEstimatedCovariance());
17666         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17667 
17668         // Force IllegalArgumentException
17669         calibrator = null;
17670         try {
17671             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17672                     measurements, true,
17673                     new Matrix(1, 1), this);
17674             fail("IllegalArgumentException expected but not thrown");
17675         } catch (final IllegalArgumentException ignore) {
17676         }
17677         try {
17678             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17679                     measurements,true,
17680                     new Matrix(1, 3), this);
17681             fail("IllegalArgumentException expected but not thrown");
17682         } catch (final IllegalArgumentException ignore) {
17683         }
17684         assertNull(calibrator);
17685     }
17686 
17687     @Test
17688     public void testConstructor149() throws WrongSizeException {
17689         final Matrix ba = generateBa();
17690         final double[] bias = ba.getBuffer();
17691         final double biasX = ba.getElementAtIndex(0);
17692         final double biasY = ba.getElementAtIndex(1);
17693         final double biasZ = ba.getElementAtIndex(2);
17694 
17695         final Matrix ma = generateMaCommonAxis();
17696         final double sx = ma.getElementAt(0, 0);
17697         final double sy = ma.getElementAt(1, 1);
17698         final double sz = ma.getElementAt(2, 2);
17699         final double mxy = ma.getElementAt(0, 1);
17700         final double mxz = ma.getElementAt(0, 2);
17701         final double myx = ma.getElementAt(1, 0);
17702         final double myz = ma.getElementAt(1, 2);
17703         final double mzx = ma.getElementAt(2, 0);
17704         final double mzy = ma.getElementAt(2, 1);
17705 
17706         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17707         final double latitude = Math.toRadians(
17708                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17709         final double longitude = Math.toRadians(
17710                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17711         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17712         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17713         final NEDVelocity nedVelocity = new NEDVelocity();
17714         final ECEFPosition ecefPosition = new ECEFPosition();
17715         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17716         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17717                 ecefPosition, ecefVelocity);
17718 
17719         KnownPositionAccelerometerCalibrator calibrator =
17720                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
17721 
17722         // check default values
17723         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17724         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17725         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17726         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17727         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17728         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17729         final Acceleration bx2 = new Acceleration(0.0,
17730                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17731         calibrator.getInitialBiasXAsAcceleration(bx2);
17732         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17733         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17734         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17735         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17736         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17737         final Acceleration by2 = new Acceleration(0.0,
17738                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17739         calibrator.getInitialBiasYAsAcceleration(by2);
17740         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17741         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17742         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17743         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17744         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17745         final Acceleration bz2 = new Acceleration(0.0,
17746                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17747         calibrator.getInitialBiasZAsAcceleration(bz2);
17748         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17749         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17750         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17751         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17752         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17753         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17754         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17755         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17756         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17757         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17758         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17759         final double[] bias1 = calibrator.getInitialBias();
17760         assertArrayEquals(bias1, bias, 0.0);
17761         final double[] bias2 = new double[3];
17762         calibrator.getInitialBias(bias2);
17763         assertArrayEquals(bias1, bias2, 0.0);
17764         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17765         assertEquals(b1, ba);
17766         final Matrix b2 = new Matrix(3, 1);
17767         calibrator.getInitialBiasAsMatrix(b2);
17768         assertEquals(b1, b2);
17769         final Matrix ma1 = new Matrix(3, 3);
17770         ma1.setSubmatrix(0, 0,
17771                 2, 2,
17772                 new double[]{ sx, myx, mzx,
17773                         mxy, sy, mzy,
17774                         mxz, myz, sz});
17775         assertEquals(calibrator.getInitialMa(), ma1);
17776         final Matrix ma2 = new Matrix(3, 3);
17777         calibrator.getInitialMa(ma2);
17778         assertEquals(ma1, ma2);
17779         assertSame(calibrator.getEcefPosition(), ecefPosition);
17780         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17781         final NEDPosition nedPosition1 = new NEDPosition();
17782         assertTrue(calibrator.getNedPosition(nedPosition1));
17783         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17784         assertNull(calibrator.getMeasurements());
17785         assertFalse(calibrator.isCommonAxisUsed());
17786         assertNull(calibrator.getListener());
17787         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17788         assertFalse(calibrator.isReady());
17789         assertFalse(calibrator.isRunning());
17790         assertNull(calibrator.getEstimatedBiases());
17791         assertFalse(calibrator.getEstimatedBiases(null));
17792         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17793         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17794         assertNull(calibrator.getEstimatedBiasFx());
17795         assertNull(calibrator.getEstimatedBiasFy());
17796         assertNull(calibrator.getEstimatedBiasFz());
17797         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17798         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17799         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17800         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17801         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17802         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17803         assertNull(calibrator.getEstimatedMa());
17804         assertNull(calibrator.getEstimatedSx());
17805         assertNull(calibrator.getEstimatedSy());
17806         assertNull(calibrator.getEstimatedSz());
17807         assertNull(calibrator.getEstimatedMxy());
17808         assertNull(calibrator.getEstimatedMxz());
17809         assertNull(calibrator.getEstimatedMyx());
17810         assertNull(calibrator.getEstimatedMyz());
17811         assertNull(calibrator.getEstimatedMzx());
17812         assertNull(calibrator.getEstimatedMzy());
17813         assertNull(calibrator.getEstimatedCovariance());
17814         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17815 
17816         // Force IllegalArgumentException
17817         calibrator = null;
17818         try {
17819             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17820                     new Matrix(1, 1), ma);
17821             fail("IllegalArgumentException expected but not thrown");
17822         } catch (final IllegalArgumentException ignore) {
17823         }
17824         try {
17825             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17826                     new Matrix(1, 3), ma);
17827             fail("IllegalArgumentException expected but not thrown");
17828         } catch (final IllegalArgumentException ignore) {
17829         }
17830         try {
17831             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17832                     ba, new Matrix(1, 3));
17833             fail("IllegalArgumentException expected but not thrown");
17834         } catch (final IllegalArgumentException ignore) {
17835         }
17836         try {
17837             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17838                     ba, new Matrix(3, 1));
17839             fail("IllegalArgumentException expected but not thrown");
17840         } catch (final IllegalArgumentException ignore) {
17841         }
17842         assertNull(calibrator);
17843     }
17844 
17845     @Test
17846     public void testConstructor150() throws WrongSizeException {
17847         final Matrix ba = generateBa();
17848         final double[] bias = ba.getBuffer();
17849         final double biasX = ba.getElementAtIndex(0);
17850         final double biasY = ba.getElementAtIndex(1);
17851         final double biasZ = ba.getElementAtIndex(2);
17852 
17853         final Matrix ma = generateMaCommonAxis();
17854         final double sx = ma.getElementAt(0, 0);
17855         final double sy = ma.getElementAt(1, 1);
17856         final double sz = ma.getElementAt(2, 2);
17857         final double mxy = ma.getElementAt(0, 1);
17858         final double mxz = ma.getElementAt(0, 2);
17859         final double myx = ma.getElementAt(1, 0);
17860         final double myz = ma.getElementAt(1, 2);
17861         final double mzx = ma.getElementAt(2, 0);
17862         final double mzy = ma.getElementAt(2, 1);
17863 
17864         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17865         final double latitude = Math.toRadians(
17866                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17867         final double longitude = Math.toRadians(
17868                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17869         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17870         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17871         final NEDVelocity nedVelocity = new NEDVelocity();
17872         final ECEFPosition ecefPosition = new ECEFPosition();
17873         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17874         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17875                 ecefPosition, ecefVelocity);
17876 
17877         KnownPositionAccelerometerCalibrator calibrator =
17878                 new KnownPositionAccelerometerCalibrator(ecefPosition,
17879                         ba, ma, this);
17880 
17881         // check default values
17882         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17883         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17884         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17885         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17886         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17887         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17888         final Acceleration bx2 = new Acceleration(0.0,
17889                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17890         calibrator.getInitialBiasXAsAcceleration(bx2);
17891         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17892         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17893         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17894         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17895         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17896         final Acceleration by2 = new Acceleration(0.0,
17897                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17898         calibrator.getInitialBiasYAsAcceleration(by2);
17899         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17900         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17901         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17902         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17903         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17904         final Acceleration bz2 = new Acceleration(0.0,
17905                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17906         calibrator.getInitialBiasZAsAcceleration(bz2);
17907         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17908         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17909         assertEquals(calibrator.getInitialSx(), sx, 0.0);
17910         assertEquals(calibrator.getInitialSy(), sy, 0.0);
17911         assertEquals(calibrator.getInitialSz(), sz, 0.0);
17912         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
17913         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
17914         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
17915         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
17916         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
17917         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
17918         final double[] bias1 = calibrator.getInitialBias();
17919         assertArrayEquals(bias1, bias, 0.0);
17920         final double[] bias2 = new double[3];
17921         calibrator.getInitialBias(bias2);
17922         assertArrayEquals(bias1, bias2, 0.0);
17923         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17924         assertEquals(b1, ba);
17925         final Matrix b2 = new Matrix(3, 1);
17926         calibrator.getInitialBiasAsMatrix(b2);
17927         assertEquals(b1, b2);
17928         final Matrix ma1 = new Matrix(3, 3);
17929         ma1.setSubmatrix(0, 0,
17930                 2, 2,
17931                 new double[]{ sx, myx, mzx,
17932                         mxy, sy, mzy,
17933                         mxz, myz, sz});
17934         assertEquals(calibrator.getInitialMa(), ma1);
17935         final Matrix ma2 = new Matrix(3, 3);
17936         calibrator.getInitialMa(ma2);
17937         assertEquals(ma1, ma2);
17938         assertSame(calibrator.getEcefPosition(), ecefPosition);
17939         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17940         final NEDPosition nedPosition1 = new NEDPosition();
17941         assertTrue(calibrator.getNedPosition(nedPosition1));
17942         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17943         assertNull(calibrator.getMeasurements());
17944         assertFalse(calibrator.isCommonAxisUsed());
17945         assertSame(calibrator.getListener(), this);
17946         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17947         assertFalse(calibrator.isReady());
17948         assertFalse(calibrator.isRunning());
17949         assertNull(calibrator.getEstimatedBiases());
17950         assertFalse(calibrator.getEstimatedBiases(null));
17951         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17952         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17953         assertNull(calibrator.getEstimatedBiasFx());
17954         assertNull(calibrator.getEstimatedBiasFy());
17955         assertNull(calibrator.getEstimatedBiasFz());
17956         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17957         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17958         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17959         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17960         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17961         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17962         assertNull(calibrator.getEstimatedMa());
17963         assertNull(calibrator.getEstimatedSx());
17964         assertNull(calibrator.getEstimatedSy());
17965         assertNull(calibrator.getEstimatedSz());
17966         assertNull(calibrator.getEstimatedMxy());
17967         assertNull(calibrator.getEstimatedMxz());
17968         assertNull(calibrator.getEstimatedMyx());
17969         assertNull(calibrator.getEstimatedMyz());
17970         assertNull(calibrator.getEstimatedMzx());
17971         assertNull(calibrator.getEstimatedMzy());
17972         assertNull(calibrator.getEstimatedCovariance());
17973         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17974 
17975         // Force IllegalArgumentException
17976         calibrator = null;
17977         try {
17978             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17979                     new Matrix(1, 1), ma, this);
17980             fail("IllegalArgumentException expected but not thrown");
17981         } catch (final IllegalArgumentException ignore) {
17982         }
17983         try {
17984             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17985                     new Matrix(1, 3), ma, this);
17986             fail("IllegalArgumentException expected but not thrown");
17987         } catch (final IllegalArgumentException ignore) {
17988         }
17989         try {
17990             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17991                     ba, new Matrix(1, 3), this);
17992             fail("IllegalArgumentException expected but not thrown");
17993         } catch (final IllegalArgumentException ignore) {
17994         }
17995         try {
17996             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17997                     ba, new Matrix(3, 1), this);
17998             fail("IllegalArgumentException expected but not thrown");
17999         } catch (final IllegalArgumentException ignore) {
18000         }
18001         assertNull(calibrator);
18002     }
18003 
18004     @Test
18005     public void testConstructor151() throws WrongSizeException {
18006         final Collection<StandardDeviationBodyKinematics> measurements =
18007                 Collections.emptyList();
18008 
18009         final Matrix ba = generateBa();
18010         final double[] bias = ba.getBuffer();
18011         final double biasX = ba.getElementAtIndex(0);
18012         final double biasY = ba.getElementAtIndex(1);
18013         final double biasZ = ba.getElementAtIndex(2);
18014 
18015         final Matrix ma = generateMaCommonAxis();
18016         final double sx = ma.getElementAt(0, 0);
18017         final double sy = ma.getElementAt(1, 1);
18018         final double sz = ma.getElementAt(2, 2);
18019         final double mxy = ma.getElementAt(0, 1);
18020         final double mxz = ma.getElementAt(0, 2);
18021         final double myx = ma.getElementAt(1, 0);
18022         final double myz = ma.getElementAt(1, 2);
18023         final double mzx = ma.getElementAt(2, 0);
18024         final double mzy = ma.getElementAt(2, 1);
18025 
18026         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18027         final double latitude = Math.toRadians(
18028                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18029         final double longitude = Math.toRadians(
18030                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18031         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18032         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18033         final NEDVelocity nedVelocity = new NEDVelocity();
18034         final ECEFPosition ecefPosition = new ECEFPosition();
18035         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18036         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18037                 ecefPosition, ecefVelocity);
18038 
18039         KnownPositionAccelerometerCalibrator calibrator =
18040                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18041                         measurements, ba, ma);
18042 
18043         // check default values
18044         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18045         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18046         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18047         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18048         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18049         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18050         final Acceleration bx2 = new Acceleration(0.0,
18051                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18052         calibrator.getInitialBiasXAsAcceleration(bx2);
18053         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18054         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18055         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18056         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18057         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18058         final Acceleration by2 = new Acceleration(0.0,
18059                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18060         calibrator.getInitialBiasYAsAcceleration(by2);
18061         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18062         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18063         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18064         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18065         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18066         final Acceleration bz2 = new Acceleration(0.0,
18067                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18068         calibrator.getInitialBiasZAsAcceleration(bz2);
18069         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18070         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18071         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18072         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18073         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18074         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18075         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18076         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18077         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18078         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18079         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18080         final double[] bias1 = calibrator.getInitialBias();
18081         assertArrayEquals(bias1, bias, 0.0);
18082         final double[] bias2 = new double[3];
18083         calibrator.getInitialBias(bias2);
18084         assertArrayEquals(bias1, bias2, 0.0);
18085         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18086         assertEquals(b1, ba);
18087         final Matrix b2 = new Matrix(3, 1);
18088         calibrator.getInitialBiasAsMatrix(b2);
18089         assertEquals(b1, b2);
18090         final Matrix ma1 = new Matrix(3, 3);
18091         ma1.setSubmatrix(0, 0,
18092                 2, 2,
18093                 new double[]{ sx, myx, mzx,
18094                         mxy, sy, mzy,
18095                         mxz, myz, sz});
18096         assertEquals(calibrator.getInitialMa(), ma1);
18097         final Matrix ma2 = new Matrix(3, 3);
18098         calibrator.getInitialMa(ma2);
18099         assertEquals(ma1, ma2);
18100         assertSame(calibrator.getEcefPosition(), ecefPosition);
18101         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18102         final NEDPosition nedPosition1 = new NEDPosition();
18103         assertTrue(calibrator.getNedPosition(nedPosition1));
18104         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18105         assertSame(calibrator.getMeasurements(), measurements);
18106         assertFalse(calibrator.isCommonAxisUsed());
18107         assertNull(calibrator.getListener());
18108         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18109         assertFalse(calibrator.isReady());
18110         assertFalse(calibrator.isRunning());
18111         assertNull(calibrator.getEstimatedBiases());
18112         assertFalse(calibrator.getEstimatedBiases(null));
18113         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18114         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18115         assertNull(calibrator.getEstimatedBiasFx());
18116         assertNull(calibrator.getEstimatedBiasFy());
18117         assertNull(calibrator.getEstimatedBiasFz());
18118         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18119         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18120         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18121         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18122         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18123         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18124         assertNull(calibrator.getEstimatedMa());
18125         assertNull(calibrator.getEstimatedSx());
18126         assertNull(calibrator.getEstimatedSy());
18127         assertNull(calibrator.getEstimatedSz());
18128         assertNull(calibrator.getEstimatedMxy());
18129         assertNull(calibrator.getEstimatedMxz());
18130         assertNull(calibrator.getEstimatedMyx());
18131         assertNull(calibrator.getEstimatedMyz());
18132         assertNull(calibrator.getEstimatedMzx());
18133         assertNull(calibrator.getEstimatedMzy());
18134         assertNull(calibrator.getEstimatedCovariance());
18135         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18136 
18137         // Force IllegalArgumentException
18138         calibrator = null;
18139         try {
18140             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18141                     measurements, new Matrix(1, 1), ma);
18142             fail("IllegalArgumentException expected but not thrown");
18143         } catch (final IllegalArgumentException ignore) {
18144         }
18145         try {
18146             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18147                     measurements, new Matrix(1, 3), ma);
18148             fail("IllegalArgumentException expected but not thrown");
18149         } catch (final IllegalArgumentException ignore) {
18150         }
18151         try {
18152             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18153                     measurements, ba, new Matrix(1, 3));
18154             fail("IllegalArgumentException expected but not thrown");
18155         } catch (final IllegalArgumentException ignore) {
18156         }
18157         try {
18158             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18159                     measurements, ba, new Matrix(3, 1));
18160             fail("IllegalArgumentException expected but not thrown");
18161         } catch (final IllegalArgumentException ignore) {
18162         }
18163         assertNull(calibrator);
18164     }
18165 
18166     @Test
18167     public void testConstructor152() throws WrongSizeException {
18168         final Collection<StandardDeviationBodyKinematics> measurements =
18169                 Collections.emptyList();
18170 
18171         final Matrix ba = generateBa();
18172         final double[] bias = ba.getBuffer();
18173         final double biasX = ba.getElementAtIndex(0);
18174         final double biasY = ba.getElementAtIndex(1);
18175         final double biasZ = ba.getElementAtIndex(2);
18176 
18177         final Matrix ma = generateMaCommonAxis();
18178         final double sx = ma.getElementAt(0, 0);
18179         final double sy = ma.getElementAt(1, 1);
18180         final double sz = ma.getElementAt(2, 2);
18181         final double mxy = ma.getElementAt(0, 1);
18182         final double mxz = ma.getElementAt(0, 2);
18183         final double myx = ma.getElementAt(1, 0);
18184         final double myz = ma.getElementAt(1, 2);
18185         final double mzx = ma.getElementAt(2, 0);
18186         final double mzy = ma.getElementAt(2, 1);
18187 
18188         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18189         final double latitude = Math.toRadians(
18190                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18191         final double longitude = Math.toRadians(
18192                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18193         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18194         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18195         final NEDVelocity nedVelocity = new NEDVelocity();
18196         final ECEFPosition ecefPosition = new ECEFPosition();
18197         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18198         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18199                 ecefPosition, ecefVelocity);
18200 
18201         KnownPositionAccelerometerCalibrator calibrator =
18202                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18203                         measurements, ba, ma, this);
18204 
18205         // check default values
18206         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18207         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18208         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18209         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18210         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18211         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18212         final Acceleration bx2 = new Acceleration(0.0,
18213                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18214         calibrator.getInitialBiasXAsAcceleration(bx2);
18215         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18216         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18217         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18218         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18219         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18220         final Acceleration by2 = new Acceleration(0.0,
18221                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18222         calibrator.getInitialBiasYAsAcceleration(by2);
18223         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18224         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18225         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18226         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18227         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18228         final Acceleration bz2 = new Acceleration(0.0,
18229                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18230         calibrator.getInitialBiasZAsAcceleration(bz2);
18231         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18232         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18233         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18234         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18235         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18236         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18237         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18238         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18239         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18240         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18241         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18242         final double[] bias1 = calibrator.getInitialBias();
18243         assertArrayEquals(bias1, bias, 0.0);
18244         final double[] bias2 = new double[3];
18245         calibrator.getInitialBias(bias2);
18246         assertArrayEquals(bias1, bias2, 0.0);
18247         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18248         assertEquals(b1, ba);
18249         final Matrix b2 = new Matrix(3, 1);
18250         calibrator.getInitialBiasAsMatrix(b2);
18251         assertEquals(b1, b2);
18252         final Matrix ma1 = new Matrix(3, 3);
18253         ma1.setSubmatrix(0, 0,
18254                 2, 2,
18255                 new double[]{ sx, myx, mzx,
18256                         mxy, sy, mzy,
18257                         mxz, myz, sz});
18258         assertEquals(calibrator.getInitialMa(), ma1);
18259         final Matrix ma2 = new Matrix(3, 3);
18260         calibrator.getInitialMa(ma2);
18261         assertEquals(ma1, ma2);
18262         assertSame(calibrator.getEcefPosition(), ecefPosition);
18263         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18264         final NEDPosition nedPosition1 = new NEDPosition();
18265         assertTrue(calibrator.getNedPosition(nedPosition1));
18266         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18267         assertSame(calibrator.getMeasurements(), measurements);
18268         assertFalse(calibrator.isCommonAxisUsed());
18269         assertSame(calibrator.getListener(), this);
18270         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18271         assertFalse(calibrator.isReady());
18272         assertFalse(calibrator.isRunning());
18273         assertNull(calibrator.getEstimatedBiases());
18274         assertFalse(calibrator.getEstimatedBiases(null));
18275         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18276         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18277         assertNull(calibrator.getEstimatedBiasFx());
18278         assertNull(calibrator.getEstimatedBiasFy());
18279         assertNull(calibrator.getEstimatedBiasFz());
18280         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18281         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18282         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18283         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18284         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18285         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18286         assertNull(calibrator.getEstimatedMa());
18287         assertNull(calibrator.getEstimatedSx());
18288         assertNull(calibrator.getEstimatedSy());
18289         assertNull(calibrator.getEstimatedSz());
18290         assertNull(calibrator.getEstimatedMxy());
18291         assertNull(calibrator.getEstimatedMxz());
18292         assertNull(calibrator.getEstimatedMyx());
18293         assertNull(calibrator.getEstimatedMyz());
18294         assertNull(calibrator.getEstimatedMzx());
18295         assertNull(calibrator.getEstimatedMzy());
18296         assertNull(calibrator.getEstimatedCovariance());
18297         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18298 
18299         // Force IllegalArgumentException
18300         calibrator = null;
18301         try {
18302             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18303                     measurements, new Matrix(1, 1), ma, this);
18304             fail("IllegalArgumentException expected but not thrown");
18305         } catch (final IllegalArgumentException ignore) {
18306         }
18307         try {
18308             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18309                     measurements, new Matrix(1, 3), ma, this);
18310             fail("IllegalArgumentException expected but not thrown");
18311         } catch (final IllegalArgumentException ignore) {
18312         }
18313         try {
18314             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18315                     measurements, ba, new Matrix(1, 3), this);
18316             fail("IllegalArgumentException expected but not thrown");
18317         } catch (final IllegalArgumentException ignore) {
18318         }
18319         try {
18320             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18321                     measurements, ba, new Matrix(3, 1), this);
18322             fail("IllegalArgumentException expected but not thrown");
18323         } catch (final IllegalArgumentException ignore) {
18324         }
18325         assertNull(calibrator);
18326     }
18327 
18328     @Test
18329     public void testConstructor153() throws WrongSizeException {
18330         final Matrix ba = generateBa();
18331         final double[] bias = ba.getBuffer();
18332         final double biasX = ba.getElementAtIndex(0);
18333         final double biasY = ba.getElementAtIndex(1);
18334         final double biasZ = ba.getElementAtIndex(2);
18335 
18336         final Matrix ma = generateMaCommonAxis();
18337         final double sx = ma.getElementAt(0, 0);
18338         final double sy = ma.getElementAt(1, 1);
18339         final double sz = ma.getElementAt(2, 2);
18340         final double mxy = ma.getElementAt(0, 1);
18341         final double mxz = ma.getElementAt(0, 2);
18342         final double myx = ma.getElementAt(1, 0);
18343         final double myz = ma.getElementAt(1, 2);
18344         final double mzx = ma.getElementAt(2, 0);
18345         final double mzy = ma.getElementAt(2, 1);
18346 
18347         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18348         final double latitude = Math.toRadians(
18349                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18350         final double longitude = Math.toRadians(
18351                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18352         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18353         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18354         final NEDVelocity nedVelocity = new NEDVelocity();
18355         final ECEFPosition ecefPosition = new ECEFPosition();
18356         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18357         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18358                 ecefPosition, ecefVelocity);
18359 
18360         KnownPositionAccelerometerCalibrator calibrator =
18361                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18362                         true, ba, ma);
18363 
18364         // check default values
18365         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18366         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18367         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18368         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18369         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18370         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18371         final Acceleration bx2 = new Acceleration(0.0,
18372                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18373         calibrator.getInitialBiasXAsAcceleration(bx2);
18374         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18375         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18376         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18377         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18378         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18379         final Acceleration by2 = new Acceleration(0.0,
18380                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18381         calibrator.getInitialBiasYAsAcceleration(by2);
18382         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18383         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18384         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18385         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18386         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18387         final Acceleration bz2 = new Acceleration(0.0,
18388                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18389         calibrator.getInitialBiasZAsAcceleration(bz2);
18390         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18391         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18392         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18393         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18394         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18395         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18396         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18397         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18398         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18399         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18400         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18401         final double[] bias1 = calibrator.getInitialBias();
18402         assertArrayEquals(bias1, bias, 0.0);
18403         final double[] bias2 = new double[3];
18404         calibrator.getInitialBias(bias2);
18405         assertArrayEquals(bias1, bias2, 0.0);
18406         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18407         assertEquals(b1, ba);
18408         final Matrix b2 = new Matrix(3, 1);
18409         calibrator.getInitialBiasAsMatrix(b2);
18410         assertEquals(b1, b2);
18411         final Matrix ma1 = new Matrix(3, 3);
18412         ma1.setSubmatrix(0, 0,
18413                 2, 2,
18414                 new double[]{ sx, myx, mzx,
18415                         mxy, sy, mzy,
18416                         mxz, myz, sz});
18417         assertEquals(calibrator.getInitialMa(), ma1);
18418         final Matrix ma2 = new Matrix(3, 3);
18419         calibrator.getInitialMa(ma2);
18420         assertEquals(ma1, ma2);
18421         assertSame(calibrator.getEcefPosition(), ecefPosition);
18422         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18423         final NEDPosition nedPosition1 = new NEDPosition();
18424         assertTrue(calibrator.getNedPosition(nedPosition1));
18425         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18426         assertNull(calibrator.getMeasurements());
18427         assertTrue(calibrator.isCommonAxisUsed());
18428         assertNull(calibrator.getListener());
18429         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18430         assertFalse(calibrator.isReady());
18431         assertFalse(calibrator.isRunning());
18432         assertNull(calibrator.getEstimatedBiases());
18433         assertFalse(calibrator.getEstimatedBiases(null));
18434         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18435         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18436         assertNull(calibrator.getEstimatedBiasFx());
18437         assertNull(calibrator.getEstimatedBiasFy());
18438         assertNull(calibrator.getEstimatedBiasFz());
18439         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18440         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18441         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18442         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18443         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18444         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18445         assertNull(calibrator.getEstimatedMa());
18446         assertNull(calibrator.getEstimatedSx());
18447         assertNull(calibrator.getEstimatedSy());
18448         assertNull(calibrator.getEstimatedSz());
18449         assertNull(calibrator.getEstimatedMxy());
18450         assertNull(calibrator.getEstimatedMxz());
18451         assertNull(calibrator.getEstimatedMyx());
18452         assertNull(calibrator.getEstimatedMyz());
18453         assertNull(calibrator.getEstimatedMzx());
18454         assertNull(calibrator.getEstimatedMzy());
18455         assertNull(calibrator.getEstimatedCovariance());
18456         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18457 
18458         // Force IllegalArgumentException
18459         calibrator = null;
18460         try {
18461             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18462                     true, new Matrix(1, 1), ma);
18463             fail("IllegalArgumentException expected but not thrown");
18464         } catch (final IllegalArgumentException ignore) {
18465         }
18466         try {
18467             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18468                     true, new Matrix(1, 3), ma);
18469             fail("IllegalArgumentException expected but not thrown");
18470         } catch (final IllegalArgumentException ignore) {
18471         }
18472         try {
18473             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18474                     true, ba, new Matrix(1, 3));
18475             fail("IllegalArgumentException expected but not thrown");
18476         } catch (final IllegalArgumentException ignore) {
18477         }
18478         try {
18479             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18480                     true, ba, new Matrix(3, 1));
18481             fail("IllegalArgumentException expected but not thrown");
18482         } catch (final IllegalArgumentException ignore) {
18483         }
18484         assertNull(calibrator);
18485     }
18486 
18487     @Test
18488     public void testConstructor154() throws WrongSizeException {
18489         final Matrix ba = generateBa();
18490         final double[] bias = ba.getBuffer();
18491         final double biasX = ba.getElementAtIndex(0);
18492         final double biasY = ba.getElementAtIndex(1);
18493         final double biasZ = ba.getElementAtIndex(2);
18494 
18495         final Matrix ma = generateMaCommonAxis();
18496         final double sx = ma.getElementAt(0, 0);
18497         final double sy = ma.getElementAt(1, 1);
18498         final double sz = ma.getElementAt(2, 2);
18499         final double mxy = ma.getElementAt(0, 1);
18500         final double mxz = ma.getElementAt(0, 2);
18501         final double myx = ma.getElementAt(1, 0);
18502         final double myz = ma.getElementAt(1, 2);
18503         final double mzx = ma.getElementAt(2, 0);
18504         final double mzy = ma.getElementAt(2, 1);
18505 
18506         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18507         final double latitude = Math.toRadians(
18508                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18509         final double longitude = Math.toRadians(
18510                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18511         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18512         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18513         final NEDVelocity nedVelocity = new NEDVelocity();
18514         final ECEFPosition ecefPosition = new ECEFPosition();
18515         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18516         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18517                 ecefPosition, ecefVelocity);
18518 
18519         KnownPositionAccelerometerCalibrator calibrator =
18520                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18521                         true, ba, ma, this);
18522 
18523         // check default values
18524         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18525         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18526         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18527         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18528         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18529         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530         final Acceleration bx2 = new Acceleration(0.0,
18531                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532         calibrator.getInitialBiasXAsAcceleration(bx2);
18533         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18534         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18536         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18537         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18538         final Acceleration by2 = new Acceleration(0.0,
18539                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18540         calibrator.getInitialBiasYAsAcceleration(by2);
18541         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18542         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18543         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18544         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18545         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18546         final Acceleration bz2 = new Acceleration(0.0,
18547                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548         calibrator.getInitialBiasZAsAcceleration(bz2);
18549         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18550         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18551         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18552         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18553         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18554         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18555         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18556         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18557         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18558         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18559         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18560         final double[] bias1 = calibrator.getInitialBias();
18561         assertArrayEquals(bias1, bias, 0.0);
18562         final double[] bias2 = new double[3];
18563         calibrator.getInitialBias(bias2);
18564         assertArrayEquals(bias1, bias2, 0.0);
18565         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18566         assertEquals(b1, ba);
18567         final Matrix b2 = new Matrix(3, 1);
18568         calibrator.getInitialBiasAsMatrix(b2);
18569         assertEquals(b1, b2);
18570         final Matrix ma1 = new Matrix(3, 3);
18571         ma1.setSubmatrix(0, 0,
18572                 2, 2,
18573                 new double[]{ sx, myx, mzx,
18574                         mxy, sy, mzy,
18575                         mxz, myz, sz});
18576         assertEquals(calibrator.getInitialMa(), ma1);
18577         final Matrix ma2 = new Matrix(3, 3);
18578         calibrator.getInitialMa(ma2);
18579         assertEquals(ma1, ma2);
18580         assertSame(calibrator.getEcefPosition(), ecefPosition);
18581         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18582         final NEDPosition nedPosition1 = new NEDPosition();
18583         assertTrue(calibrator.getNedPosition(nedPosition1));
18584         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18585         assertNull(calibrator.getMeasurements());
18586         assertTrue(calibrator.isCommonAxisUsed());
18587         assertSame(calibrator.getListener(), this);
18588         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18589         assertFalse(calibrator.isReady());
18590         assertFalse(calibrator.isRunning());
18591         assertNull(calibrator.getEstimatedBiases());
18592         assertFalse(calibrator.getEstimatedBiases(null));
18593         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18594         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18595         assertNull(calibrator.getEstimatedBiasFx());
18596         assertNull(calibrator.getEstimatedBiasFy());
18597         assertNull(calibrator.getEstimatedBiasFz());
18598         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18599         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18600         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18601         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18602         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18603         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18604         assertNull(calibrator.getEstimatedMa());
18605         assertNull(calibrator.getEstimatedSx());
18606         assertNull(calibrator.getEstimatedSy());
18607         assertNull(calibrator.getEstimatedSz());
18608         assertNull(calibrator.getEstimatedMxy());
18609         assertNull(calibrator.getEstimatedMxz());
18610         assertNull(calibrator.getEstimatedMyx());
18611         assertNull(calibrator.getEstimatedMyz());
18612         assertNull(calibrator.getEstimatedMzx());
18613         assertNull(calibrator.getEstimatedMzy());
18614         assertNull(calibrator.getEstimatedCovariance());
18615         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18616 
18617         // Force IllegalArgumentException
18618         calibrator = null;
18619         try {
18620             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18621                     true, new Matrix(1, 1), ma, this);
18622             fail("IllegalArgumentException expected but not thrown");
18623         } catch (final IllegalArgumentException ignore) {
18624         }
18625         try {
18626             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18627                     true, new Matrix(1, 3), ma, this);
18628             fail("IllegalArgumentException expected but not thrown");
18629         } catch (final IllegalArgumentException ignore) {
18630         }
18631         try {
18632             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18633                     true, ba, new Matrix(1, 3), this);
18634             fail("IllegalArgumentException expected but not thrown");
18635         } catch (final IllegalArgumentException ignore) {
18636         }
18637         try {
18638             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18639                     true, ba, new Matrix(3, 1), this);
18640             fail("IllegalArgumentException expected but not thrown");
18641         } catch (final IllegalArgumentException ignore) {
18642         }
18643         assertNull(calibrator);
18644     }
18645 
18646     @Test
18647     public void testConstructor155() throws WrongSizeException {
18648         final Collection<StandardDeviationBodyKinematics> measurements =
18649                 Collections.emptyList();
18650 
18651         final Matrix ba = generateBa();
18652         final double[] bias = ba.getBuffer();
18653         final double biasX = ba.getElementAtIndex(0);
18654         final double biasY = ba.getElementAtIndex(1);
18655         final double biasZ = ba.getElementAtIndex(2);
18656 
18657         final Matrix ma = generateMaCommonAxis();
18658         final double sx = ma.getElementAt(0, 0);
18659         final double sy = ma.getElementAt(1, 1);
18660         final double sz = ma.getElementAt(2, 2);
18661         final double mxy = ma.getElementAt(0, 1);
18662         final double mxz = ma.getElementAt(0, 2);
18663         final double myx = ma.getElementAt(1, 0);
18664         final double myz = ma.getElementAt(1, 2);
18665         final double mzx = ma.getElementAt(2, 0);
18666         final double mzy = ma.getElementAt(2, 1);
18667 
18668         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18669         final double latitude = Math.toRadians(
18670                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18671         final double longitude = Math.toRadians(
18672                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18673         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18674         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18675         final NEDVelocity nedVelocity = new NEDVelocity();
18676         final ECEFPosition ecefPosition = new ECEFPosition();
18677         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18678         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18679                 ecefPosition, ecefVelocity);
18680 
18681         KnownPositionAccelerometerCalibrator calibrator =
18682                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18683                         true, ba, ma);
18684 
18685         // check default values
18686         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18687         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18688         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18689         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18690         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18691         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18692         final Acceleration bx2 = new Acceleration(0.0,
18693                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18694         calibrator.getInitialBiasXAsAcceleration(bx2);
18695         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18696         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18697         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18698         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18699         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18700         final Acceleration by2 = new Acceleration(0.0,
18701                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18702         calibrator.getInitialBiasYAsAcceleration(by2);
18703         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18704         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18705         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18706         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18707         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18708         final Acceleration bz2 = new Acceleration(0.0,
18709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18710         calibrator.getInitialBiasZAsAcceleration(bz2);
18711         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18712         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18713         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18714         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18715         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18716         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18717         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18718         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18719         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18720         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18721         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18722         final double[] bias1 = calibrator.getInitialBias();
18723         assertArrayEquals(bias1, bias, 0.0);
18724         final double[] bias2 = new double[3];
18725         calibrator.getInitialBias(bias2);
18726         assertArrayEquals(bias1, bias2, 0.0);
18727         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18728         assertEquals(b1, ba);
18729         final Matrix b2 = new Matrix(3, 1);
18730         calibrator.getInitialBiasAsMatrix(b2);
18731         assertEquals(b1, b2);
18732         final Matrix ma1 = new Matrix(3, 3);
18733         ma1.setSubmatrix(0, 0,
18734                 2, 2,
18735                 new double[]{ sx, myx, mzx,
18736                         mxy, sy, mzy,
18737                         mxz, myz, sz});
18738         assertEquals(calibrator.getInitialMa(), ma1);
18739         final Matrix ma2 = new Matrix(3, 3);
18740         calibrator.getInitialMa(ma2);
18741         assertEquals(ma1, ma2);
18742         assertSame(calibrator.getEcefPosition(), ecefPosition);
18743         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18744         final NEDPosition nedPosition1 = new NEDPosition();
18745         assertTrue(calibrator.getNedPosition(nedPosition1));
18746         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18747         assertSame(calibrator.getMeasurements(), measurements);
18748         assertTrue(calibrator.isCommonAxisUsed());
18749         assertNull(calibrator.getListener());
18750         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18751         assertFalse(calibrator.isReady());
18752         assertFalse(calibrator.isRunning());
18753         assertNull(calibrator.getEstimatedBiases());
18754         assertFalse(calibrator.getEstimatedBiases(null));
18755         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18756         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18757         assertNull(calibrator.getEstimatedBiasFx());
18758         assertNull(calibrator.getEstimatedBiasFy());
18759         assertNull(calibrator.getEstimatedBiasFz());
18760         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18761         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18762         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18763         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18764         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18765         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18766         assertNull(calibrator.getEstimatedMa());
18767         assertNull(calibrator.getEstimatedSx());
18768         assertNull(calibrator.getEstimatedSy());
18769         assertNull(calibrator.getEstimatedSz());
18770         assertNull(calibrator.getEstimatedMxy());
18771         assertNull(calibrator.getEstimatedMxz());
18772         assertNull(calibrator.getEstimatedMyx());
18773         assertNull(calibrator.getEstimatedMyz());
18774         assertNull(calibrator.getEstimatedMzx());
18775         assertNull(calibrator.getEstimatedMzy());
18776         assertNull(calibrator.getEstimatedCovariance());
18777         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18778 
18779         // Force IllegalArgumentException
18780         calibrator = null;
18781         try {
18782             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18783                     measurements, true,
18784                     new Matrix(1, 1), ma);
18785             fail("IllegalArgumentException expected but not thrown");
18786         } catch (final IllegalArgumentException ignore) {
18787         }
18788         try {
18789             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18790                     measurements, true,
18791                     new Matrix(1, 3), ma);
18792             fail("IllegalArgumentException expected but not thrown");
18793         } catch (final IllegalArgumentException ignore) {
18794         }
18795         try {
18796             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18797                     measurements, true,
18798                     ba, new Matrix(1, 3));
18799             fail("IllegalArgumentException expected but not thrown");
18800         } catch (final IllegalArgumentException ignore) {
18801         }
18802         try {
18803             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18804                     measurements, true,
18805                     ba, new Matrix(3, 1));
18806             fail("IllegalArgumentException expected but not thrown");
18807         } catch (final IllegalArgumentException ignore) {
18808         }
18809         assertNull(calibrator);
18810     }
18811 
18812     @Test
18813     public void testConstructor156() throws WrongSizeException {
18814         final Collection<StandardDeviationBodyKinematics> measurements =
18815                 Collections.emptyList();
18816 
18817         final Matrix ba = generateBa();
18818         final double[] bias = ba.getBuffer();
18819         final double biasX = ba.getElementAtIndex(0);
18820         final double biasY = ba.getElementAtIndex(1);
18821         final double biasZ = ba.getElementAtIndex(2);
18822 
18823         final Matrix ma = generateMaCommonAxis();
18824         final double sx = ma.getElementAt(0, 0);
18825         final double sy = ma.getElementAt(1, 1);
18826         final double sz = ma.getElementAt(2, 2);
18827         final double mxy = ma.getElementAt(0, 1);
18828         final double mxz = ma.getElementAt(0, 2);
18829         final double myx = ma.getElementAt(1, 0);
18830         final double myz = ma.getElementAt(1, 2);
18831         final double mzx = ma.getElementAt(2, 0);
18832         final double mzy = ma.getElementAt(2, 1);
18833 
18834         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18835         final double latitude = Math.toRadians(
18836                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18837         final double longitude = Math.toRadians(
18838                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18839         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18840         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18841         final NEDVelocity nedVelocity = new NEDVelocity();
18842         final ECEFPosition ecefPosition = new ECEFPosition();
18843         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18844         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18845                 ecefPosition, ecefVelocity);
18846 
18847         KnownPositionAccelerometerCalibrator calibrator =
18848                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18849                         true, ba, ma, this);
18850 
18851         // check default values
18852         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18853         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18854         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18855         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18856         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18857         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18858         final Acceleration bx2 = new Acceleration(0.0,
18859                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18860         calibrator.getInitialBiasXAsAcceleration(bx2);
18861         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18862         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18863         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18864         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18865         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18866         final Acceleration by2 = new Acceleration(0.0,
18867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18868         calibrator.getInitialBiasYAsAcceleration(by2);
18869         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18870         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18871         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18872         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18873         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18874         final Acceleration bz2 = new Acceleration(0.0,
18875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18876         calibrator.getInitialBiasZAsAcceleration(bz2);
18877         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18878         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18879         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18880         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18881         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18882         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18883         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18884         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18885         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18886         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18887         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18888         final double[] bias1 = calibrator.getInitialBias();
18889         assertArrayEquals(bias1, bias, 0.0);
18890         final double[] bias2 = new double[3];
18891         calibrator.getInitialBias(bias2);
18892         assertArrayEquals(bias1, bias2, 0.0);
18893         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18894         assertEquals(b1, ba);
18895         final Matrix b2 = new Matrix(3, 1);
18896         calibrator.getInitialBiasAsMatrix(b2);
18897         assertEquals(b1, b2);
18898         final Matrix ma1 = new Matrix(3, 3);
18899         ma1.setSubmatrix(0, 0,
18900                 2, 2,
18901                 new double[]{ sx, myx, mzx,
18902                         mxy, sy, mzy,
18903                         mxz, myz, sz});
18904         assertEquals(calibrator.getInitialMa(), ma1);
18905         final Matrix ma2 = new Matrix(3, 3);
18906         calibrator.getInitialMa(ma2);
18907         assertEquals(ma1, ma2);
18908         assertSame(calibrator.getEcefPosition(), ecefPosition);
18909         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18910         final NEDPosition nedPosition1 = new NEDPosition();
18911         assertTrue(calibrator.getNedPosition(nedPosition1));
18912         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18913         assertSame(calibrator.getMeasurements(), measurements);
18914         assertTrue(calibrator.isCommonAxisUsed());
18915         assertSame(calibrator.getListener(), this);
18916         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18917         assertFalse(calibrator.isReady());
18918         assertFalse(calibrator.isRunning());
18919         assertNull(calibrator.getEstimatedBiases());
18920         assertFalse(calibrator.getEstimatedBiases(null));
18921         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18922         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18923         assertNull(calibrator.getEstimatedBiasFx());
18924         assertNull(calibrator.getEstimatedBiasFy());
18925         assertNull(calibrator.getEstimatedBiasFz());
18926         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18927         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18928         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18929         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18930         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18931         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18932         assertNull(calibrator.getEstimatedMa());
18933         assertNull(calibrator.getEstimatedSx());
18934         assertNull(calibrator.getEstimatedSy());
18935         assertNull(calibrator.getEstimatedSz());
18936         assertNull(calibrator.getEstimatedMxy());
18937         assertNull(calibrator.getEstimatedMxz());
18938         assertNull(calibrator.getEstimatedMyx());
18939         assertNull(calibrator.getEstimatedMyz());
18940         assertNull(calibrator.getEstimatedMzx());
18941         assertNull(calibrator.getEstimatedMzy());
18942         assertNull(calibrator.getEstimatedCovariance());
18943         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18944 
18945         // Force IllegalArgumentException
18946         calibrator = null;
18947         try {
18948             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18949                     measurements, true,
18950                     new Matrix(1, 1), ma, this);
18951             fail("IllegalArgumentException expected but not thrown");
18952         } catch (final IllegalArgumentException ignore) {
18953         }
18954         try {
18955             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18956                     measurements, true,
18957                     new Matrix(1, 3), ma, this);
18958             fail("IllegalArgumentException expected but not thrown");
18959         } catch (final IllegalArgumentException ignore) {
18960         }
18961         try {
18962             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18963                     measurements, true,
18964                     ba, new Matrix(1, 3), this);
18965             fail("IllegalArgumentException expected but not thrown");
18966         } catch (final IllegalArgumentException ignore) {
18967         }
18968         try {
18969             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18970                     measurements, true,
18971                     ba, new Matrix(3, 1), this);
18972             fail("IllegalArgumentException expected but not thrown");
18973         } catch (final IllegalArgumentException ignore) {
18974         }
18975         assertNull(calibrator);
18976     }
18977 
18978     @Test
18979     public void testConstructor157() throws WrongSizeException {
18980         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18981         final double latitude = Math.toRadians(
18982                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18983         final double longitude = Math.toRadians(
18984                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18985         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18986         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18987         final NEDVelocity nedVelocity = new NEDVelocity();
18988         final ECEFPosition ecefPosition = new ECEFPosition();
18989         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18990         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18991                 ecefPosition, ecefVelocity);
18992 
18993         final KnownPositionAccelerometerCalibrator calibrator =
18994                 new KnownPositionAccelerometerCalibrator(nedPosition);
18995 
18996         // check default values
18997         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
18998         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
18999         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19000         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19001         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19002         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19003         final Acceleration bx2 = new Acceleration(0.0,
19004                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19005         calibrator.getInitialBiasXAsAcceleration(bx2);
19006         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19007         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19008         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19009         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19010         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19011         final Acceleration by2 = new Acceleration(0.0,
19012                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19013         calibrator.getInitialBiasYAsAcceleration(by2);
19014         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19015         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19016         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19017         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19018         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19019         final Acceleration bz2 = new Acceleration(0.0,
19020                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19021         calibrator.getInitialBiasZAsAcceleration(bz2);
19022         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19023         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19024         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19025         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19026         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19027         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19028         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19029         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19030         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19031         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19032         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19033         final double[] bias1 = calibrator.getInitialBias();
19034         assertArrayEquals(bias1, new double[3], 0.0);
19035         final double[] bias2 = new double[3];
19036         calibrator.getInitialBias(bias2);
19037         assertArrayEquals(bias1, bias2, 0.0);
19038         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19039         assertEquals(b1, new Matrix(3, 1));
19040         final Matrix b2 = new Matrix(3, 1);
19041         calibrator.getInitialBiasAsMatrix(b2);
19042         assertEquals(b1, b2);
19043         final Matrix ma1 = calibrator.getInitialMa();
19044         assertEquals(ma1, new Matrix(3, 3));
19045         final Matrix ma2 = new Matrix(3, 3);
19046         calibrator.getInitialMa(ma2);
19047         assertEquals(ma1, ma2);
19048         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19049         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19050         final NEDPosition nedPosition1 = new NEDPosition();
19051         assertTrue(calibrator.getNedPosition(nedPosition1));
19052         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19053         assertNull(calibrator.getMeasurements());
19054         assertFalse(calibrator.isCommonAxisUsed());
19055         assertNull(calibrator.getListener());
19056         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19057         assertFalse(calibrator.isReady());
19058         assertFalse(calibrator.isRunning());
19059         assertNull(calibrator.getEstimatedBiases());
19060         assertFalse(calibrator.getEstimatedBiases(null));
19061         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19062         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19063         assertNull(calibrator.getEstimatedBiasFx());
19064         assertNull(calibrator.getEstimatedBiasFy());
19065         assertNull(calibrator.getEstimatedBiasFz());
19066         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19067         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19068         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19069         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19070         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19071         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19072         assertNull(calibrator.getEstimatedMa());
19073         assertNull(calibrator.getEstimatedSx());
19074         assertNull(calibrator.getEstimatedSy());
19075         assertNull(calibrator.getEstimatedSz());
19076         assertNull(calibrator.getEstimatedMxy());
19077         assertNull(calibrator.getEstimatedMxz());
19078         assertNull(calibrator.getEstimatedMyx());
19079         assertNull(calibrator.getEstimatedMyz());
19080         assertNull(calibrator.getEstimatedMzx());
19081         assertNull(calibrator.getEstimatedMzy());
19082         assertNull(calibrator.getEstimatedCovariance());
19083         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19084     }
19085 
19086     @Test
19087     public void testConstructor158() throws WrongSizeException {
19088         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19089         final double latitude = Math.toRadians(
19090                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19091         final double longitude = Math.toRadians(
19092                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19093         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19094         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19095         final NEDVelocity nedVelocity = new NEDVelocity();
19096         final ECEFPosition ecefPosition = new ECEFPosition();
19097         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19098         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19099                 ecefPosition, ecefVelocity);
19100 
19101         final KnownPositionAccelerometerCalibrator calibrator =
19102                 new KnownPositionAccelerometerCalibrator(nedPosition, this);
19103 
19104         // check default values
19105         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19106         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19107         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19108         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19109         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19110         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19111         final Acceleration bx2 = new Acceleration(0.0,
19112                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19113         calibrator.getInitialBiasXAsAcceleration(bx2);
19114         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19115         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19116         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19117         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19118         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19119         final Acceleration by2 = new Acceleration(0.0,
19120                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19121         calibrator.getInitialBiasYAsAcceleration(by2);
19122         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19123         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19124         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19125         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19126         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19127         final Acceleration bz2 = new Acceleration(0.0,
19128                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19129         calibrator.getInitialBiasZAsAcceleration(bz2);
19130         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19131         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19132         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19133         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19134         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19135         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19136         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19137         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19138         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19139         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19140         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19141         final double[] bias1 = calibrator.getInitialBias();
19142         assertArrayEquals(bias1, new double[3], 0.0);
19143         final double[] bias2 = new double[3];
19144         calibrator.getInitialBias(bias2);
19145         assertArrayEquals(bias1, bias2, 0.0);
19146         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19147         assertEquals(b1, new Matrix(3, 1));
19148         final Matrix b2 = new Matrix(3, 1);
19149         calibrator.getInitialBiasAsMatrix(b2);
19150         assertEquals(b1, b2);
19151         final Matrix ma1 = calibrator.getInitialMa();
19152         assertEquals(ma1, new Matrix(3, 3));
19153         final Matrix ma2 = new Matrix(3, 3);
19154         calibrator.getInitialMa(ma2);
19155         assertEquals(ma1, ma2);
19156         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19157         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19158         final NEDPosition nedPosition1 = new NEDPosition();
19159         assertTrue(calibrator.getNedPosition(nedPosition1));
19160         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19161         assertNull(calibrator.getMeasurements());
19162         assertFalse(calibrator.isCommonAxisUsed());
19163         assertSame(calibrator.getListener(), this);
19164         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19165         assertFalse(calibrator.isReady());
19166         assertFalse(calibrator.isRunning());
19167         assertNull(calibrator.getEstimatedBiases());
19168         assertFalse(calibrator.getEstimatedBiases(null));
19169         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19170         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19171         assertNull(calibrator.getEstimatedBiasFx());
19172         assertNull(calibrator.getEstimatedBiasFy());
19173         assertNull(calibrator.getEstimatedBiasFz());
19174         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19175         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19176         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19177         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19178         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19179         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19180         assertNull(calibrator.getEstimatedMa());
19181         assertNull(calibrator.getEstimatedSx());
19182         assertNull(calibrator.getEstimatedSy());
19183         assertNull(calibrator.getEstimatedSz());
19184         assertNull(calibrator.getEstimatedMxy());
19185         assertNull(calibrator.getEstimatedMxz());
19186         assertNull(calibrator.getEstimatedMyx());
19187         assertNull(calibrator.getEstimatedMyz());
19188         assertNull(calibrator.getEstimatedMzx());
19189         assertNull(calibrator.getEstimatedMzy());
19190         assertNull(calibrator.getEstimatedCovariance());
19191         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19192     }
19193 
19194     @Test
19195     public void testConstructor159() throws WrongSizeException {
19196         final Collection<StandardDeviationBodyKinematics> measurements =
19197                 Collections.emptyList();
19198 
19199         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19200         final double latitude = Math.toRadians(
19201                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19202         final double longitude = Math.toRadians(
19203                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19204         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19205         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19206         final NEDVelocity nedVelocity = new NEDVelocity();
19207         final ECEFPosition ecefPosition = new ECEFPosition();
19208         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19209         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19210                 ecefPosition, ecefVelocity);
19211 
19212         final KnownPositionAccelerometerCalibrator calibrator =
19213                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
19214 
19215         // check default values
19216         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19217         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19218         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19219         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19220         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19221         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19222         final Acceleration bx2 = new Acceleration(0.0,
19223                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19224         calibrator.getInitialBiasXAsAcceleration(bx2);
19225         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19226         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19227         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19228         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19229         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19230         final Acceleration by2 = new Acceleration(0.0,
19231                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19232         calibrator.getInitialBiasYAsAcceleration(by2);
19233         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19234         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19235         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19236         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19237         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19238         final Acceleration bz2 = new Acceleration(0.0,
19239                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19240         calibrator.getInitialBiasZAsAcceleration(bz2);
19241         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19242         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19243         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19244         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19245         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19246         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19247         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19248         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19249         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19250         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19251         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19252         final double[] bias1 = calibrator.getInitialBias();
19253         assertArrayEquals(bias1, new double[3], 0.0);
19254         final double[] bias2 = new double[3];
19255         calibrator.getInitialBias(bias2);
19256         assertArrayEquals(bias1, bias2, 0.0);
19257         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19258         assertEquals(b1, new Matrix(3, 1));
19259         final Matrix b2 = new Matrix(3, 1);
19260         calibrator.getInitialBiasAsMatrix(b2);
19261         assertEquals(b1, b2);
19262         final Matrix ma1 = calibrator.getInitialMa();
19263         assertEquals(ma1, new Matrix(3, 3));
19264         final Matrix ma2 = new Matrix(3, 3);
19265         calibrator.getInitialMa(ma2);
19266         assertEquals(ma1, ma2);
19267         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19268         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19269         final NEDPosition nedPosition1 = new NEDPosition();
19270         assertTrue(calibrator.getNedPosition(nedPosition1));
19271         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19272         assertSame(calibrator.getMeasurements(), measurements);
19273         assertFalse(calibrator.isCommonAxisUsed());
19274         assertNull(calibrator.getListener());
19275         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19276         assertFalse(calibrator.isReady());
19277         assertFalse(calibrator.isRunning());
19278         assertNull(calibrator.getEstimatedBiases());
19279         assertFalse(calibrator.getEstimatedBiases(null));
19280         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19281         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19282         assertNull(calibrator.getEstimatedBiasFx());
19283         assertNull(calibrator.getEstimatedBiasFy());
19284         assertNull(calibrator.getEstimatedBiasFz());
19285         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19286         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19287         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19288         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19289         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19290         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19291         assertNull(calibrator.getEstimatedMa());
19292         assertNull(calibrator.getEstimatedSx());
19293         assertNull(calibrator.getEstimatedSy());
19294         assertNull(calibrator.getEstimatedSz());
19295         assertNull(calibrator.getEstimatedMxy());
19296         assertNull(calibrator.getEstimatedMxz());
19297         assertNull(calibrator.getEstimatedMyx());
19298         assertNull(calibrator.getEstimatedMyz());
19299         assertNull(calibrator.getEstimatedMzx());
19300         assertNull(calibrator.getEstimatedMzy());
19301         assertNull(calibrator.getEstimatedCovariance());
19302         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19303     }
19304 
19305     @Test
19306     public void testConstructor160() throws WrongSizeException {
19307         final Collection<StandardDeviationBodyKinematics> measurements =
19308                 Collections.emptyList();
19309 
19310         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19311         final double latitude = Math.toRadians(
19312                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19313         final double longitude = Math.toRadians(
19314                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19315         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19316         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19317         final NEDVelocity nedVelocity = new NEDVelocity();
19318         final ECEFPosition ecefPosition = new ECEFPosition();
19319         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19320         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19321                 ecefPosition, ecefVelocity);
19322 
19323         final KnownPositionAccelerometerCalibrator calibrator =
19324                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19325                         this);
19326 
19327         // check default values
19328         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19329         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19330         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19331         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19332         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19333         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19334         final Acceleration bx2 = new Acceleration(0.0,
19335                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19336         calibrator.getInitialBiasXAsAcceleration(bx2);
19337         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19338         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19339         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19340         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19341         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19342         final Acceleration by2 = new Acceleration(0.0,
19343                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19344         calibrator.getInitialBiasYAsAcceleration(by2);
19345         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19346         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19347         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19348         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19349         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19350         final Acceleration bz2 = new Acceleration(0.0,
19351                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19352         calibrator.getInitialBiasZAsAcceleration(bz2);
19353         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19354         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19355         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19356         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19357         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19358         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19359         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19360         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19361         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19362         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19363         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19364         final double[] bias1 = calibrator.getInitialBias();
19365         assertArrayEquals(bias1, new double[3], 0.0);
19366         final double[] bias2 = new double[3];
19367         calibrator.getInitialBias(bias2);
19368         assertArrayEquals(bias1, bias2, 0.0);
19369         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19370         assertEquals(b1, new Matrix(3, 1));
19371         final Matrix b2 = new Matrix(3, 1);
19372         calibrator.getInitialBiasAsMatrix(b2);
19373         assertEquals(b1, b2);
19374         final Matrix ma1 = calibrator.getInitialMa();
19375         assertEquals(ma1, new Matrix(3, 3));
19376         final Matrix ma2 = new Matrix(3, 3);
19377         calibrator.getInitialMa(ma2);
19378         assertEquals(ma1, ma2);
19379         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19380         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19381         final NEDPosition nedPosition1 = new NEDPosition();
19382         assertTrue(calibrator.getNedPosition(nedPosition1));
19383         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19384         assertSame(calibrator.getMeasurements(), measurements);
19385         assertFalse(calibrator.isCommonAxisUsed());
19386         assertSame(calibrator.getListener(), this);
19387         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19388         assertFalse(calibrator.isReady());
19389         assertFalse(calibrator.isRunning());
19390         assertNull(calibrator.getEstimatedBiases());
19391         assertFalse(calibrator.getEstimatedBiases(null));
19392         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19393         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19394         assertNull(calibrator.getEstimatedBiasFx());
19395         assertNull(calibrator.getEstimatedBiasFy());
19396         assertNull(calibrator.getEstimatedBiasFz());
19397         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19398         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19399         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19400         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19401         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19402         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19403         assertNull(calibrator.getEstimatedMa());
19404         assertNull(calibrator.getEstimatedSx());
19405         assertNull(calibrator.getEstimatedSy());
19406         assertNull(calibrator.getEstimatedSz());
19407         assertNull(calibrator.getEstimatedMxy());
19408         assertNull(calibrator.getEstimatedMxz());
19409         assertNull(calibrator.getEstimatedMyx());
19410         assertNull(calibrator.getEstimatedMyz());
19411         assertNull(calibrator.getEstimatedMzx());
19412         assertNull(calibrator.getEstimatedMzy());
19413         assertNull(calibrator.getEstimatedCovariance());
19414         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19415     }
19416 
19417     @Test
19418     public void testConstructor161() throws WrongSizeException {
19419         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19420         final double latitude = Math.toRadians(
19421                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19422         final double longitude = Math.toRadians(
19423                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19424         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19425         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19426         final NEDVelocity nedVelocity = new NEDVelocity();
19427         final ECEFPosition ecefPosition = new ECEFPosition();
19428         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19429         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19430                 ecefPosition, ecefVelocity);
19431 
19432         final KnownPositionAccelerometerCalibrator calibrator =
19433                 new KnownPositionAccelerometerCalibrator(nedPosition,
19434                         true);
19435 
19436         // check default values
19437         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19438         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19439         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19440         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19441         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19442         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19443         final Acceleration bx2 = new Acceleration(0.0,
19444                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19445         calibrator.getInitialBiasXAsAcceleration(bx2);
19446         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19447         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19448         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19449         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19450         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19451         final Acceleration by2 = new Acceleration(0.0,
19452                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19453         calibrator.getInitialBiasYAsAcceleration(by2);
19454         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19455         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19456         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19457         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19458         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19459         final Acceleration bz2 = new Acceleration(0.0,
19460                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19461         calibrator.getInitialBiasZAsAcceleration(bz2);
19462         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19463         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19464         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19465         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19466         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19467         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19468         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19469         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19470         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19471         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19472         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19473         final double[] bias1 = calibrator.getInitialBias();
19474         assertArrayEquals(bias1, new double[3], 0.0);
19475         final double[] bias2 = new double[3];
19476         calibrator.getInitialBias(bias2);
19477         assertArrayEquals(bias1, bias2, 0.0);
19478         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19479         assertEquals(b1, new Matrix(3, 1));
19480         final Matrix b2 = new Matrix(3, 1);
19481         calibrator.getInitialBiasAsMatrix(b2);
19482         assertEquals(b1, b2);
19483         final Matrix ma1 = calibrator.getInitialMa();
19484         assertEquals(ma1, new Matrix(3, 3));
19485         final Matrix ma2 = new Matrix(3, 3);
19486         calibrator.getInitialMa(ma2);
19487         assertEquals(ma1, ma2);
19488         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19489         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19490         final NEDPosition nedPosition1 = new NEDPosition();
19491         assertTrue(calibrator.getNedPosition(nedPosition1));
19492         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19493         assertNull(calibrator.getMeasurements());
19494         assertTrue(calibrator.isCommonAxisUsed());
19495         assertNull(calibrator.getListener());
19496         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19497         assertFalse(calibrator.isReady());
19498         assertFalse(calibrator.isRunning());
19499         assertNull(calibrator.getEstimatedBiases());
19500         assertFalse(calibrator.getEstimatedBiases(null));
19501         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19502         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19503         assertNull(calibrator.getEstimatedBiasFx());
19504         assertNull(calibrator.getEstimatedBiasFy());
19505         assertNull(calibrator.getEstimatedBiasFz());
19506         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19507         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19508         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19509         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19510         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19511         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19512         assertNull(calibrator.getEstimatedMa());
19513         assertNull(calibrator.getEstimatedSx());
19514         assertNull(calibrator.getEstimatedSy());
19515         assertNull(calibrator.getEstimatedSz());
19516         assertNull(calibrator.getEstimatedMxy());
19517         assertNull(calibrator.getEstimatedMxz());
19518         assertNull(calibrator.getEstimatedMyx());
19519         assertNull(calibrator.getEstimatedMyz());
19520         assertNull(calibrator.getEstimatedMzx());
19521         assertNull(calibrator.getEstimatedMzy());
19522         assertNull(calibrator.getEstimatedCovariance());
19523         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19524     }
19525 
19526     @Test
19527     public void testConstructor162() throws WrongSizeException {
19528         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19529         final double latitude = Math.toRadians(
19530                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19531         final double longitude = Math.toRadians(
19532                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19533         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19534         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19535         final NEDVelocity nedVelocity = new NEDVelocity();
19536         final ECEFPosition ecefPosition = new ECEFPosition();
19537         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19538         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19539                 ecefPosition, ecefVelocity);
19540 
19541         final KnownPositionAccelerometerCalibrator calibrator =
19542                 new KnownPositionAccelerometerCalibrator(nedPosition,
19543                         true, this);
19544 
19545         // check default values
19546         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19547         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19548         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19549         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19550         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19551         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19552         final Acceleration bx2 = new Acceleration(0.0,
19553                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19554         calibrator.getInitialBiasXAsAcceleration(bx2);
19555         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19556         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19557         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19558         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19559         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19560         final Acceleration by2 = new Acceleration(0.0,
19561                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19562         calibrator.getInitialBiasYAsAcceleration(by2);
19563         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19564         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19565         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19566         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19567         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19568         final Acceleration bz2 = new Acceleration(0.0,
19569                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19570         calibrator.getInitialBiasZAsAcceleration(bz2);
19571         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19572         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19573         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19574         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19575         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19576         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19577         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19578         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19579         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19580         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19581         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19582         final double[] bias1 = calibrator.getInitialBias();
19583         assertArrayEquals(bias1, new double[3], 0.0);
19584         final double[] bias2 = new double[3];
19585         calibrator.getInitialBias(bias2);
19586         assertArrayEquals(bias1, bias2, 0.0);
19587         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19588         assertEquals(b1, new Matrix(3, 1));
19589         final Matrix b2 = new Matrix(3, 1);
19590         calibrator.getInitialBiasAsMatrix(b2);
19591         assertEquals(b1, b2);
19592         final Matrix ma1 = calibrator.getInitialMa();
19593         assertEquals(ma1, new Matrix(3, 3));
19594         final Matrix ma2 = new Matrix(3, 3);
19595         calibrator.getInitialMa(ma2);
19596         assertEquals(ma1, ma2);
19597         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19598         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19599         final NEDPosition nedPosition1 = new NEDPosition();
19600         assertTrue(calibrator.getNedPosition(nedPosition1));
19601         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19602         assertNull(calibrator.getMeasurements());
19603         assertTrue(calibrator.isCommonAxisUsed());
19604         assertSame(calibrator.getListener(), this);
19605         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19606         assertFalse(calibrator.isReady());
19607         assertFalse(calibrator.isRunning());
19608         assertNull(calibrator.getEstimatedBiases());
19609         assertFalse(calibrator.getEstimatedBiases(null));
19610         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19611         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19612         assertNull(calibrator.getEstimatedBiasFx());
19613         assertNull(calibrator.getEstimatedBiasFy());
19614         assertNull(calibrator.getEstimatedBiasFz());
19615         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19616         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19617         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19618         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19619         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19620         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19621         assertNull(calibrator.getEstimatedMa());
19622         assertNull(calibrator.getEstimatedSx());
19623         assertNull(calibrator.getEstimatedSy());
19624         assertNull(calibrator.getEstimatedSz());
19625         assertNull(calibrator.getEstimatedMxy());
19626         assertNull(calibrator.getEstimatedMxz());
19627         assertNull(calibrator.getEstimatedMyx());
19628         assertNull(calibrator.getEstimatedMyz());
19629         assertNull(calibrator.getEstimatedMzx());
19630         assertNull(calibrator.getEstimatedMzy());
19631         assertNull(calibrator.getEstimatedCovariance());
19632         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19633     }
19634 
19635     @Test
19636     public void testConstructor163() throws WrongSizeException {
19637         final Collection<StandardDeviationBodyKinematics> measurements =
19638                 Collections.emptyList();
19639         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19640         final double latitude = Math.toRadians(
19641                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19642         final double longitude = Math.toRadians(
19643                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19644         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19645         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19646         final NEDVelocity nedVelocity = new NEDVelocity();
19647         final ECEFPosition ecefPosition = new ECEFPosition();
19648         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19649         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19650                 ecefPosition, ecefVelocity);
19651 
19652         final KnownPositionAccelerometerCalibrator calibrator =
19653                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19654                         true);
19655 
19656         // check default values
19657         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19658         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19659         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19660         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19661         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19662         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19663         final Acceleration bx2 = new Acceleration(0.0,
19664                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19665         calibrator.getInitialBiasXAsAcceleration(bx2);
19666         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19667         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19668         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19669         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19670         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19671         final Acceleration by2 = new Acceleration(0.0,
19672                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19673         calibrator.getInitialBiasYAsAcceleration(by2);
19674         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19675         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19676         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19677         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19678         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19679         final Acceleration bz2 = new Acceleration(0.0,
19680                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19681         calibrator.getInitialBiasZAsAcceleration(bz2);
19682         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19683         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19684         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19685         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19686         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19687         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19688         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19689         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19690         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19691         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19692         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19693         final double[] bias1 = calibrator.getInitialBias();
19694         assertArrayEquals(bias1, new double[3], 0.0);
19695         final double[] bias2 = new double[3];
19696         calibrator.getInitialBias(bias2);
19697         assertArrayEquals(bias1, bias2, 0.0);
19698         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19699         assertEquals(b1, new Matrix(3, 1));
19700         final Matrix b2 = new Matrix(3, 1);
19701         calibrator.getInitialBiasAsMatrix(b2);
19702         assertEquals(b1, b2);
19703         final Matrix ma1 = calibrator.getInitialMa();
19704         assertEquals(ma1, new Matrix(3, 3));
19705         final Matrix ma2 = new Matrix(3, 3);
19706         calibrator.getInitialMa(ma2);
19707         assertEquals(ma1, ma2);
19708         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19709         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19710         final NEDPosition nedPosition1 = new NEDPosition();
19711         assertTrue(calibrator.getNedPosition(nedPosition1));
19712         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19713         assertSame(calibrator.getMeasurements(), measurements);
19714         assertTrue(calibrator.isCommonAxisUsed());
19715         assertNull(calibrator.getListener());
19716         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19717         assertFalse(calibrator.isReady());
19718         assertFalse(calibrator.isRunning());
19719         assertNull(calibrator.getEstimatedBiases());
19720         assertFalse(calibrator.getEstimatedBiases(null));
19721         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19722         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19723         assertNull(calibrator.getEstimatedBiasFx());
19724         assertNull(calibrator.getEstimatedBiasFy());
19725         assertNull(calibrator.getEstimatedBiasFz());
19726         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19727         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19728         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19729         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19730         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19731         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19732         assertNull(calibrator.getEstimatedMa());
19733         assertNull(calibrator.getEstimatedSx());
19734         assertNull(calibrator.getEstimatedSy());
19735         assertNull(calibrator.getEstimatedSz());
19736         assertNull(calibrator.getEstimatedMxy());
19737         assertNull(calibrator.getEstimatedMxz());
19738         assertNull(calibrator.getEstimatedMyx());
19739         assertNull(calibrator.getEstimatedMyz());
19740         assertNull(calibrator.getEstimatedMzx());
19741         assertNull(calibrator.getEstimatedMzy());
19742         assertNull(calibrator.getEstimatedCovariance());
19743         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19744     }
19745 
19746     @Test
19747     public void testConstructor164() throws WrongSizeException {
19748         final Collection<StandardDeviationBodyKinematics> measurements =
19749                 Collections.emptyList();
19750         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19751         final double latitude = Math.toRadians(
19752                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19753         final double longitude = Math.toRadians(
19754                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19755         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19756         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19757         final NEDVelocity nedVelocity = new NEDVelocity();
19758         final ECEFPosition ecefPosition = new ECEFPosition();
19759         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19760         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19761                 ecefPosition, ecefVelocity);
19762 
19763         final KnownPositionAccelerometerCalibrator calibrator =
19764                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
19765                         true, this);
19766 
19767         // check default values
19768         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
19769         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
19770         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
19771         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19772         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
19773         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19774         final Acceleration bx2 = new Acceleration(0.0,
19775                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19776         calibrator.getInitialBiasXAsAcceleration(bx2);
19777         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
19778         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19779         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19780         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
19781         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19782         final Acceleration by2 = new Acceleration(0.0,
19783                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19784         calibrator.getInitialBiasYAsAcceleration(by2);
19785         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
19786         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19787         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19788         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
19789         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19790         final Acceleration bz2 = new Acceleration(0.0,
19791                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19792         calibrator.getInitialBiasZAsAcceleration(bz2);
19793         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
19794         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19795         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19796         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19797         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19798         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19799         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19800         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19801         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19802         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19803         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19804         final double[] bias1 = calibrator.getInitialBias();
19805         assertArrayEquals(bias1, new double[3], 0.0);
19806         final double[] bias2 = new double[3];
19807         calibrator.getInitialBias(bias2);
19808         assertArrayEquals(bias1, bias2, 0.0);
19809         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19810         assertEquals(b1, new Matrix(3, 1));
19811         final Matrix b2 = new Matrix(3, 1);
19812         calibrator.getInitialBiasAsMatrix(b2);
19813         assertEquals(b1, b2);
19814         final Matrix ma1 = calibrator.getInitialMa();
19815         assertEquals(ma1, new Matrix(3, 3));
19816         final Matrix ma2 = new Matrix(3, 3);
19817         calibrator.getInitialMa(ma2);
19818         assertEquals(ma1, ma2);
19819         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19820         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19821         final NEDPosition nedPosition1 = new NEDPosition();
19822         assertTrue(calibrator.getNedPosition(nedPosition1));
19823         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19824         assertSame(calibrator.getMeasurements(), measurements);
19825         assertTrue(calibrator.isCommonAxisUsed());
19826         assertSame(calibrator.getListener(), this);
19827         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19828         assertFalse(calibrator.isReady());
19829         assertFalse(calibrator.isRunning());
19830         assertNull(calibrator.getEstimatedBiases());
19831         assertFalse(calibrator.getEstimatedBiases(null));
19832         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19833         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19834         assertNull(calibrator.getEstimatedBiasFx());
19835         assertNull(calibrator.getEstimatedBiasFy());
19836         assertNull(calibrator.getEstimatedBiasFz());
19837         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19838         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19839         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19840         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19841         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19842         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19843         assertNull(calibrator.getEstimatedMa());
19844         assertNull(calibrator.getEstimatedSx());
19845         assertNull(calibrator.getEstimatedSy());
19846         assertNull(calibrator.getEstimatedSz());
19847         assertNull(calibrator.getEstimatedMxy());
19848         assertNull(calibrator.getEstimatedMxz());
19849         assertNull(calibrator.getEstimatedMyx());
19850         assertNull(calibrator.getEstimatedMyz());
19851         assertNull(calibrator.getEstimatedMzx());
19852         assertNull(calibrator.getEstimatedMzy());
19853         assertNull(calibrator.getEstimatedCovariance());
19854         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19855     }
19856 
19857     @Test
19858     public void testConstructor165() throws WrongSizeException {
19859         final Matrix ba = generateBa();
19860         final double biasX = ba.getElementAtIndex(0);
19861         final double biasY = ba.getElementAtIndex(1);
19862         final double biasZ = ba.getElementAtIndex(2);
19863 
19864         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19865         final double latitude = Math.toRadians(
19866                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19867         final double longitude = Math.toRadians(
19868                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19869         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19870         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19871         final NEDVelocity nedVelocity = new NEDVelocity();
19872         final ECEFPosition ecefPosition = new ECEFPosition();
19873         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19874         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19875                 ecefPosition, ecefVelocity);
19876 
19877         final KnownPositionAccelerometerCalibrator calibrator =
19878                 new KnownPositionAccelerometerCalibrator(nedPosition,
19879                         biasX, biasY, biasZ);
19880 
19881         // check default values
19882         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19883         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19884         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19885         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19886         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19887         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19888         final Acceleration bx2 = new Acceleration(0.0,
19889                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19890         calibrator.getInitialBiasXAsAcceleration(bx2);
19891         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19892         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19893         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19894         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19895         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19896         final Acceleration by2 = new Acceleration(0.0,
19897                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19898         calibrator.getInitialBiasYAsAcceleration(by2);
19899         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19900         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19901         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19902         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19903         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19904         final Acceleration bz2 = new Acceleration(0.0,
19905                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19906         calibrator.getInitialBiasZAsAcceleration(bz2);
19907         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19908         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19909         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
19910         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
19911         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
19912         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
19913         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
19914         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
19915         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
19916         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
19917         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
19918         final double[] bias1 = calibrator.getInitialBias();
19919         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
19920         final double[] bias2 = new double[3];
19921         calibrator.getInitialBias(bias2);
19922         assertArrayEquals(bias1, bias2, 0.0);
19923         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19924         assertEquals(b1, ba);
19925         final Matrix b2 = new Matrix(3, 1);
19926         calibrator.getInitialBiasAsMatrix(b2);
19927         assertEquals(b1, b2);
19928         final Matrix ma1 = calibrator.getInitialMa();
19929         assertEquals(ma1, new Matrix(3, 3));
19930         final Matrix ma2 = new Matrix(3, 3);
19931         calibrator.getInitialMa(ma2);
19932         assertEquals(ma1, ma2);
19933         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
19934         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19935         final NEDPosition nedPosition1 = new NEDPosition();
19936         assertTrue(calibrator.getNedPosition(nedPosition1));
19937         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19938         assertNull(calibrator.getMeasurements());
19939         assertFalse(calibrator.isCommonAxisUsed());
19940         assertNull(calibrator.getListener());
19941         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19942         assertFalse(calibrator.isReady());
19943         assertFalse(calibrator.isRunning());
19944         assertNull(calibrator.getEstimatedBiases());
19945         assertFalse(calibrator.getEstimatedBiases(null));
19946         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19947         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19948         assertNull(calibrator.getEstimatedBiasFx());
19949         assertNull(calibrator.getEstimatedBiasFy());
19950         assertNull(calibrator.getEstimatedBiasFz());
19951         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19952         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19953         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19954         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19955         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19956         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19957         assertNull(calibrator.getEstimatedMa());
19958         assertNull(calibrator.getEstimatedSx());
19959         assertNull(calibrator.getEstimatedSy());
19960         assertNull(calibrator.getEstimatedSz());
19961         assertNull(calibrator.getEstimatedMxy());
19962         assertNull(calibrator.getEstimatedMxz());
19963         assertNull(calibrator.getEstimatedMyx());
19964         assertNull(calibrator.getEstimatedMyz());
19965         assertNull(calibrator.getEstimatedMzx());
19966         assertNull(calibrator.getEstimatedMzy());
19967         assertNull(calibrator.getEstimatedCovariance());
19968         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19969     }
19970 
19971     @Test
19972     public void testConstructor166() throws WrongSizeException {
19973         final Matrix ba = generateBa();
19974         final double biasX = ba.getElementAtIndex(0);
19975         final double biasY = ba.getElementAtIndex(1);
19976         final double biasZ = ba.getElementAtIndex(2);
19977 
19978         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19979         final double latitude = Math.toRadians(
19980                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19981         final double longitude = Math.toRadians(
19982                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19983         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19984         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19985         final NEDVelocity nedVelocity = new NEDVelocity();
19986         final ECEFPosition ecefPosition = new ECEFPosition();
19987         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19988         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19989                 ecefPosition, ecefVelocity);
19990 
19991         final KnownPositionAccelerometerCalibrator calibrator =
19992                 new KnownPositionAccelerometerCalibrator(nedPosition,
19993                         biasX, biasY, biasZ, this);
19994 
19995         // check default values
19996         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19997         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19998         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19999         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20000         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20001         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20002         final Acceleration bx2 = new Acceleration(0.0,
20003                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20004         calibrator.getInitialBiasXAsAcceleration(bx2);
20005         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20006         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20007         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20008         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20009         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20010         final Acceleration by2 = new Acceleration(0.0,
20011                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20012         calibrator.getInitialBiasYAsAcceleration(by2);
20013         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20014         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20015         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20016         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20017         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20018         final Acceleration bz2 = new Acceleration(0.0,
20019                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20020         calibrator.getInitialBiasZAsAcceleration(bz2);
20021         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20022         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20023         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20024         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20025         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20026         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20027         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20028         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20029         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20030         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20031         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20032         final double[] bias1 = calibrator.getInitialBias();
20033         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20034         final double[] bias2 = new double[3];
20035         calibrator.getInitialBias(bias2);
20036         assertArrayEquals(bias1, bias2, 0.0);
20037         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20038         assertEquals(b1, ba);
20039         final Matrix b2 = new Matrix(3, 1);
20040         calibrator.getInitialBiasAsMatrix(b2);
20041         assertEquals(b1, b2);
20042         final Matrix ma1 = calibrator.getInitialMa();
20043         assertEquals(ma1, new Matrix(3, 3));
20044         final Matrix ma2 = new Matrix(3, 3);
20045         calibrator.getInitialMa(ma2);
20046         assertEquals(ma1, ma2);
20047         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20048         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20049         final NEDPosition nedPosition1 = new NEDPosition();
20050         assertTrue(calibrator.getNedPosition(nedPosition1));
20051         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20052         assertNull(calibrator.getMeasurements());
20053         assertFalse(calibrator.isCommonAxisUsed());
20054         assertSame(calibrator.getListener(), this);
20055         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20056         assertFalse(calibrator.isReady());
20057         assertFalse(calibrator.isRunning());
20058         assertNull(calibrator.getEstimatedBiases());
20059         assertFalse(calibrator.getEstimatedBiases(null));
20060         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20061         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20062         assertNull(calibrator.getEstimatedBiasFx());
20063         assertNull(calibrator.getEstimatedBiasFy());
20064         assertNull(calibrator.getEstimatedBiasFz());
20065         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20066         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20067         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20068         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20069         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20070         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20071         assertNull(calibrator.getEstimatedMa());
20072         assertNull(calibrator.getEstimatedSx());
20073         assertNull(calibrator.getEstimatedSy());
20074         assertNull(calibrator.getEstimatedSz());
20075         assertNull(calibrator.getEstimatedMxy());
20076         assertNull(calibrator.getEstimatedMxz());
20077         assertNull(calibrator.getEstimatedMyx());
20078         assertNull(calibrator.getEstimatedMyz());
20079         assertNull(calibrator.getEstimatedMzx());
20080         assertNull(calibrator.getEstimatedMzy());
20081         assertNull(calibrator.getEstimatedCovariance());
20082         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20083     }
20084 
20085     @Test
20086     public void testConstructor167() throws WrongSizeException {
20087         final Collection<StandardDeviationBodyKinematics> measurements =
20088                 Collections.emptyList();
20089 
20090         final Matrix ba = generateBa();
20091         final double biasX = ba.getElementAtIndex(0);
20092         final double biasY = ba.getElementAtIndex(1);
20093         final double biasZ = ba.getElementAtIndex(2);
20094 
20095         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20096         final double latitude = Math.toRadians(
20097                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20098         final double longitude = Math.toRadians(
20099                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20100         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20101         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20102         final NEDVelocity nedVelocity = new NEDVelocity();
20103         final ECEFPosition ecefPosition = new ECEFPosition();
20104         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20105         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20106                 ecefPosition, ecefVelocity);
20107 
20108         final KnownPositionAccelerometerCalibrator calibrator =
20109                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20110                         biasX, biasY, biasZ);
20111 
20112         // check default values
20113         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20114         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20115         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20116         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20117         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20118         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20119         final Acceleration bx2 = new Acceleration(0.0,
20120                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20121         calibrator.getInitialBiasXAsAcceleration(bx2);
20122         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20123         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20124         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20125         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20126         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20127         final Acceleration by2 = new Acceleration(0.0,
20128                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20129         calibrator.getInitialBiasYAsAcceleration(by2);
20130         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20131         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20132         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20133         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20134         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20135         final Acceleration bz2 = new Acceleration(0.0,
20136                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20137         calibrator.getInitialBiasZAsAcceleration(bz2);
20138         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20139         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20140         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20141         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20142         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20143         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20144         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20145         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20146         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20147         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20148         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20149         final double[] bias1 = calibrator.getInitialBias();
20150         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20151         final double[] bias2 = new double[3];
20152         calibrator.getInitialBias(bias2);
20153         assertArrayEquals(bias1, bias2, 0.0);
20154         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20155         assertEquals(b1, ba);
20156         final Matrix b2 = new Matrix(3, 1);
20157         calibrator.getInitialBiasAsMatrix(b2);
20158         assertEquals(b1, b2);
20159         final Matrix ma1 = calibrator.getInitialMa();
20160         assertEquals(ma1, new Matrix(3, 3));
20161         final Matrix ma2 = new Matrix(3, 3);
20162         calibrator.getInitialMa(ma2);
20163         assertEquals(ma1, ma2);
20164         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20165         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20166         final NEDPosition nedPosition1 = new NEDPosition();
20167         assertTrue(calibrator.getNedPosition(nedPosition1));
20168         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20169         assertSame(calibrator.getMeasurements(), measurements);
20170         assertFalse(calibrator.isCommonAxisUsed());
20171         assertNull(calibrator.getListener());
20172         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20173         assertFalse(calibrator.isReady());
20174         assertFalse(calibrator.isRunning());
20175         assertNull(calibrator.getEstimatedBiases());
20176         assertFalse(calibrator.getEstimatedBiases(null));
20177         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20178         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20179         assertNull(calibrator.getEstimatedBiasFx());
20180         assertNull(calibrator.getEstimatedBiasFy());
20181         assertNull(calibrator.getEstimatedBiasFz());
20182         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20183         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20184         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20185         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20186         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20187         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20188         assertNull(calibrator.getEstimatedMa());
20189         assertNull(calibrator.getEstimatedSx());
20190         assertNull(calibrator.getEstimatedSy());
20191         assertNull(calibrator.getEstimatedSz());
20192         assertNull(calibrator.getEstimatedMxy());
20193         assertNull(calibrator.getEstimatedMxz());
20194         assertNull(calibrator.getEstimatedMyx());
20195         assertNull(calibrator.getEstimatedMyz());
20196         assertNull(calibrator.getEstimatedMzx());
20197         assertNull(calibrator.getEstimatedMzy());
20198         assertNull(calibrator.getEstimatedCovariance());
20199         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20200     }
20201 
20202     @Test
20203     public void testConstructor168() throws WrongSizeException {
20204         final Collection<StandardDeviationBodyKinematics> measurements =
20205                 Collections.emptyList();
20206 
20207         final Matrix ba = generateBa();
20208         final double biasX = ba.getElementAtIndex(0);
20209         final double biasY = ba.getElementAtIndex(1);
20210         final double biasZ = ba.getElementAtIndex(2);
20211 
20212         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20213         final double latitude = Math.toRadians(
20214                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20215         final double longitude = Math.toRadians(
20216                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20217         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20218         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20219         final NEDVelocity nedVelocity = new NEDVelocity();
20220         final ECEFPosition ecefPosition = new ECEFPosition();
20221         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20222         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20223                 ecefPosition, ecefVelocity);
20224 
20225         final KnownPositionAccelerometerCalibrator calibrator =
20226                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20227                         biasX, biasY, biasZ, this);
20228 
20229         // check default values
20230         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20231         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20232         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20233         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20234         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20235         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20236         final Acceleration bx2 = new Acceleration(0.0,
20237                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20238         calibrator.getInitialBiasXAsAcceleration(bx2);
20239         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20240         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20241         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20242         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20243         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20244         final Acceleration by2 = new Acceleration(0.0,
20245                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20246         calibrator.getInitialBiasYAsAcceleration(by2);
20247         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20248         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20249         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20250         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20251         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20252         final Acceleration bz2 = new Acceleration(0.0,
20253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20254         calibrator.getInitialBiasZAsAcceleration(bz2);
20255         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20256         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20257         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20258         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20259         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20260         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20261         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20262         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20263         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20264         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20265         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20266         final double[] bias1 = calibrator.getInitialBias();
20267         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20268         final double[] bias2 = new double[3];
20269         calibrator.getInitialBias(bias2);
20270         assertArrayEquals(bias1, bias2, 0.0);
20271         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20272         assertEquals(b1, ba);
20273         final Matrix b2 = new Matrix(3, 1);
20274         calibrator.getInitialBiasAsMatrix(b2);
20275         assertEquals(b1, b2);
20276         final Matrix ma1 = calibrator.getInitialMa();
20277         assertEquals(ma1, new Matrix(3, 3));
20278         final Matrix ma2 = new Matrix(3, 3);
20279         calibrator.getInitialMa(ma2);
20280         assertEquals(ma1, ma2);
20281         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20282         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20283         final NEDPosition nedPosition1 = new NEDPosition();
20284         assertTrue(calibrator.getNedPosition(nedPosition1));
20285         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20286         assertSame(calibrator.getMeasurements(), measurements);
20287         assertFalse(calibrator.isCommonAxisUsed());
20288         assertSame(calibrator.getListener(), this);
20289         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20290         assertFalse(calibrator.isReady());
20291         assertFalse(calibrator.isRunning());
20292         assertNull(calibrator.getEstimatedBiases());
20293         assertFalse(calibrator.getEstimatedBiases(null));
20294         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20295         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20296         assertNull(calibrator.getEstimatedBiasFx());
20297         assertNull(calibrator.getEstimatedBiasFy());
20298         assertNull(calibrator.getEstimatedBiasFz());
20299         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20300         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20301         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20302         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20303         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20304         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20305         assertNull(calibrator.getEstimatedMa());
20306         assertNull(calibrator.getEstimatedSx());
20307         assertNull(calibrator.getEstimatedSy());
20308         assertNull(calibrator.getEstimatedSz());
20309         assertNull(calibrator.getEstimatedMxy());
20310         assertNull(calibrator.getEstimatedMxz());
20311         assertNull(calibrator.getEstimatedMyx());
20312         assertNull(calibrator.getEstimatedMyz());
20313         assertNull(calibrator.getEstimatedMzx());
20314         assertNull(calibrator.getEstimatedMzy());
20315         assertNull(calibrator.getEstimatedCovariance());
20316         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20317     }
20318 
20319     @Test
20320     public void testConstructor169() throws WrongSizeException {
20321         final Matrix ba = generateBa();
20322         final double biasX = ba.getElementAtIndex(0);
20323         final double biasY = ba.getElementAtIndex(1);
20324         final double biasZ = ba.getElementAtIndex(2);
20325 
20326         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20327         final double latitude = Math.toRadians(
20328                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20329         final double longitude = Math.toRadians(
20330                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20331         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20332         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20333         final NEDVelocity nedVelocity = new NEDVelocity();
20334         final ECEFPosition ecefPosition = new ECEFPosition();
20335         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20336         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20337                 ecefPosition, ecefVelocity);
20338 
20339         final KnownPositionAccelerometerCalibrator calibrator =
20340                 new KnownPositionAccelerometerCalibrator(nedPosition,
20341                         true, biasX, biasY, biasZ);
20342 
20343         // check default values
20344         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20345         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20346         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20347         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20348         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20349         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20350         final Acceleration bx2 = new Acceleration(0.0,
20351                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20352         calibrator.getInitialBiasXAsAcceleration(bx2);
20353         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20354         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20355         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20356         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20357         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20358         final Acceleration by2 = new Acceleration(0.0,
20359                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20360         calibrator.getInitialBiasYAsAcceleration(by2);
20361         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20362         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20363         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20364         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20365         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20366         final Acceleration bz2 = new Acceleration(0.0,
20367                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20368         calibrator.getInitialBiasZAsAcceleration(bz2);
20369         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20370         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20371         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20372         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20373         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20374         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20375         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20376         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20377         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20378         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20379         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20380         final double[] bias1 = calibrator.getInitialBias();
20381         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20382         final double[] bias2 = new double[3];
20383         calibrator.getInitialBias(bias2);
20384         assertArrayEquals(bias1, bias2, 0.0);
20385         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20386         assertEquals(b1, ba);
20387         final Matrix b2 = new Matrix(3, 1);
20388         calibrator.getInitialBiasAsMatrix(b2);
20389         assertEquals(b1, b2);
20390         final Matrix ma1 = calibrator.getInitialMa();
20391         assertEquals(ma1, new Matrix(3, 3));
20392         final Matrix ma2 = new Matrix(3, 3);
20393         calibrator.getInitialMa(ma2);
20394         assertEquals(ma1, ma2);
20395         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20396         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20397         final NEDPosition nedPosition1 = new NEDPosition();
20398         assertTrue(calibrator.getNedPosition(nedPosition1));
20399         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20400         assertNull(calibrator.getMeasurements());
20401         assertTrue(calibrator.isCommonAxisUsed());
20402         assertNull(calibrator.getListener());
20403         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20404         assertFalse(calibrator.isReady());
20405         assertFalse(calibrator.isRunning());
20406         assertNull(calibrator.getEstimatedBiases());
20407         assertFalse(calibrator.getEstimatedBiases(null));
20408         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20409         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20410         assertNull(calibrator.getEstimatedBiasFx());
20411         assertNull(calibrator.getEstimatedBiasFy());
20412         assertNull(calibrator.getEstimatedBiasFz());
20413         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20414         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20415         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20416         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20417         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20418         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20419         assertNull(calibrator.getEstimatedMa());
20420         assertNull(calibrator.getEstimatedSx());
20421         assertNull(calibrator.getEstimatedSy());
20422         assertNull(calibrator.getEstimatedSz());
20423         assertNull(calibrator.getEstimatedMxy());
20424         assertNull(calibrator.getEstimatedMxz());
20425         assertNull(calibrator.getEstimatedMyx());
20426         assertNull(calibrator.getEstimatedMyz());
20427         assertNull(calibrator.getEstimatedMzx());
20428         assertNull(calibrator.getEstimatedMzy());
20429         assertNull(calibrator.getEstimatedCovariance());
20430         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20431     }
20432 
20433     @Test
20434     public void testConstructor170() throws WrongSizeException {
20435         final Matrix ba = generateBa();
20436         final double biasX = ba.getElementAtIndex(0);
20437         final double biasY = ba.getElementAtIndex(1);
20438         final double biasZ = ba.getElementAtIndex(2);
20439 
20440         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20441         final double latitude = Math.toRadians(
20442                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20443         final double longitude = Math.toRadians(
20444                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20445         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20446         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20447         final NEDVelocity nedVelocity = new NEDVelocity();
20448         final ECEFPosition ecefPosition = new ECEFPosition();
20449         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20450         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20451                 ecefPosition, ecefVelocity);
20452 
20453         final KnownPositionAccelerometerCalibrator calibrator =
20454                 new KnownPositionAccelerometerCalibrator(nedPosition,
20455                         true, biasX, biasY, biasZ, this);
20456 
20457         // check default values
20458         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20459         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20460         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20461         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20462         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20463         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20464         final Acceleration bx2 = new Acceleration(0.0,
20465                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20466         calibrator.getInitialBiasXAsAcceleration(bx2);
20467         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20468         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20469         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20470         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20471         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20472         final Acceleration by2 = new Acceleration(0.0,
20473                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20474         calibrator.getInitialBiasYAsAcceleration(by2);
20475         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20476         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20477         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20478         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20479         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20480         final Acceleration bz2 = new Acceleration(0.0,
20481                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20482         calibrator.getInitialBiasZAsAcceleration(bz2);
20483         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20484         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20485         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20486         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20487         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20488         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20489         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20490         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20491         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20492         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20493         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20494         final double[] bias1 = calibrator.getInitialBias();
20495         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20496         final double[] bias2 = new double[3];
20497         calibrator.getInitialBias(bias2);
20498         assertArrayEquals(bias1, bias2, 0.0);
20499         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20500         assertEquals(b1, ba);
20501         final Matrix b2 = new Matrix(3, 1);
20502         calibrator.getInitialBiasAsMatrix(b2);
20503         assertEquals(b1, b2);
20504         final Matrix ma1 = calibrator.getInitialMa();
20505         assertEquals(ma1, new Matrix(3, 3));
20506         final Matrix ma2 = new Matrix(3, 3);
20507         calibrator.getInitialMa(ma2);
20508         assertEquals(ma1, ma2);
20509         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20510         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20511         final NEDPosition nedPosition1 = new NEDPosition();
20512         assertTrue(calibrator.getNedPosition(nedPosition1));
20513         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20514         assertNull(calibrator.getMeasurements());
20515         assertTrue(calibrator.isCommonAxisUsed());
20516         assertSame(calibrator.getListener(), this);
20517         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20518         assertFalse(calibrator.isReady());
20519         assertFalse(calibrator.isRunning());
20520         assertNull(calibrator.getEstimatedBiases());
20521         assertFalse(calibrator.getEstimatedBiases(null));
20522         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20523         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20524         assertNull(calibrator.getEstimatedBiasFx());
20525         assertNull(calibrator.getEstimatedBiasFy());
20526         assertNull(calibrator.getEstimatedBiasFz());
20527         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20528         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20529         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20530         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20531         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20532         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20533         assertNull(calibrator.getEstimatedMa());
20534         assertNull(calibrator.getEstimatedSx());
20535         assertNull(calibrator.getEstimatedSy());
20536         assertNull(calibrator.getEstimatedSz());
20537         assertNull(calibrator.getEstimatedMxy());
20538         assertNull(calibrator.getEstimatedMxz());
20539         assertNull(calibrator.getEstimatedMyx());
20540         assertNull(calibrator.getEstimatedMyz());
20541         assertNull(calibrator.getEstimatedMzx());
20542         assertNull(calibrator.getEstimatedMzy());
20543         assertNull(calibrator.getEstimatedCovariance());
20544         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20545     }
20546 
20547     @Test
20548     public void testConstructor171() throws WrongSizeException {
20549         final Collection<StandardDeviationBodyKinematics> measurements =
20550                 Collections.emptyList();
20551 
20552         final Matrix ba = generateBa();
20553         final double biasX = ba.getElementAtIndex(0);
20554         final double biasY = ba.getElementAtIndex(1);
20555         final double biasZ = ba.getElementAtIndex(2);
20556 
20557         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20558         final double latitude = Math.toRadians(
20559                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20560         final double longitude = Math.toRadians(
20561                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20562         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20563         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20564         final NEDVelocity nedVelocity = new NEDVelocity();
20565         final ECEFPosition ecefPosition = new ECEFPosition();
20566         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20567         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20568                 ecefPosition, ecefVelocity);
20569 
20570         final KnownPositionAccelerometerCalibrator calibrator =
20571                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20572                         true, biasX, biasY, biasZ);
20573 
20574         // check default values
20575         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20576         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20577         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20578         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20579         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20580         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20581         final Acceleration bx2 = new Acceleration(0.0,
20582                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20583         calibrator.getInitialBiasXAsAcceleration(bx2);
20584         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20585         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20586         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20587         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20588         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20589         final Acceleration by2 = new Acceleration(0.0,
20590                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20591         calibrator.getInitialBiasYAsAcceleration(by2);
20592         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20593         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20594         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20595         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20596         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20597         final Acceleration bz2 = new Acceleration(0.0,
20598                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20599         calibrator.getInitialBiasZAsAcceleration(bz2);
20600         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20601         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20602         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20603         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20604         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20605         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20606         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20607         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20608         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20609         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20610         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20611         final double[] bias1 = calibrator.getInitialBias();
20612         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20613         final double[] bias2 = new double[3];
20614         calibrator.getInitialBias(bias2);
20615         assertArrayEquals(bias1, bias2, 0.0);
20616         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20617         assertEquals(b1, ba);
20618         final Matrix b2 = new Matrix(3, 1);
20619         calibrator.getInitialBiasAsMatrix(b2);
20620         assertEquals(b1, b2);
20621         final Matrix ma1 = calibrator.getInitialMa();
20622         assertEquals(ma1, new Matrix(3, 3));
20623         final Matrix ma2 = new Matrix(3, 3);
20624         calibrator.getInitialMa(ma2);
20625         assertEquals(ma1, ma2);
20626         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20627         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20628         final NEDPosition nedPosition1 = new NEDPosition();
20629         assertTrue(calibrator.getNedPosition(nedPosition1));
20630         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20631         assertSame(calibrator.getMeasurements(), measurements);
20632         assertTrue(calibrator.isCommonAxisUsed());
20633         assertNull(calibrator.getListener());
20634         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20635         assertFalse(calibrator.isReady());
20636         assertFalse(calibrator.isRunning());
20637         assertNull(calibrator.getEstimatedBiases());
20638         assertFalse(calibrator.getEstimatedBiases(null));
20639         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20640         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20641         assertNull(calibrator.getEstimatedBiasFx());
20642         assertNull(calibrator.getEstimatedBiasFy());
20643         assertNull(calibrator.getEstimatedBiasFz());
20644         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20645         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20646         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20647         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20648         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20649         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20650         assertNull(calibrator.getEstimatedMa());
20651         assertNull(calibrator.getEstimatedSx());
20652         assertNull(calibrator.getEstimatedSy());
20653         assertNull(calibrator.getEstimatedSz());
20654         assertNull(calibrator.getEstimatedMxy());
20655         assertNull(calibrator.getEstimatedMxz());
20656         assertNull(calibrator.getEstimatedMyx());
20657         assertNull(calibrator.getEstimatedMyz());
20658         assertNull(calibrator.getEstimatedMzx());
20659         assertNull(calibrator.getEstimatedMzy());
20660         assertNull(calibrator.getEstimatedCovariance());
20661         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20662     }
20663 
20664     @Test
20665     public void testConstructor172() throws WrongSizeException {
20666         final Collection<StandardDeviationBodyKinematics> measurements =
20667                 Collections.emptyList();
20668 
20669         final Matrix ba = generateBa();
20670         final double biasX = ba.getElementAtIndex(0);
20671         final double biasY = ba.getElementAtIndex(1);
20672         final double biasZ = ba.getElementAtIndex(2);
20673 
20674         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20675         final double latitude = Math.toRadians(
20676                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20677         final double longitude = Math.toRadians(
20678                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20679         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20680         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20681         final NEDVelocity nedVelocity = new NEDVelocity();
20682         final ECEFPosition ecefPosition = new ECEFPosition();
20683         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20684         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20685                 ecefPosition, ecefVelocity);
20686 
20687         final KnownPositionAccelerometerCalibrator calibrator =
20688                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20689                         true, biasX, biasY, biasZ, this);
20690 
20691         // check default values
20692         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20693         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20694         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20695         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20696         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20697         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20698         final Acceleration bx2 = new Acceleration(0.0,
20699                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20700         calibrator.getInitialBiasXAsAcceleration(bx2);
20701         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20702         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20703         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20704         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20705         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706         final Acceleration by2 = new Acceleration(0.0,
20707                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20708         calibrator.getInitialBiasYAsAcceleration(by2);
20709         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20710         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20711         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20712         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20713         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20714         final Acceleration bz2 = new Acceleration(0.0,
20715                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20716         calibrator.getInitialBiasZAsAcceleration(bz2);
20717         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20718         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20719         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20720         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20721         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20722         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20723         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20724         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20725         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20726         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20727         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20728         final double[] bias1 = calibrator.getInitialBias();
20729         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20730         final double[] bias2 = new double[3];
20731         calibrator.getInitialBias(bias2);
20732         assertArrayEquals(bias1, bias2, 0.0);
20733         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20734         assertEquals(b1, ba);
20735         final Matrix b2 = new Matrix(3, 1);
20736         calibrator.getInitialBiasAsMatrix(b2);
20737         assertEquals(b1, b2);
20738         final Matrix ma1 = calibrator.getInitialMa();
20739         assertEquals(ma1, new Matrix(3, 3));
20740         final Matrix ma2 = new Matrix(3, 3);
20741         calibrator.getInitialMa(ma2);
20742         assertEquals(ma1, ma2);
20743         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20744         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20745         final NEDPosition nedPosition1 = new NEDPosition();
20746         assertTrue(calibrator.getNedPosition(nedPosition1));
20747         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20748         assertSame(calibrator.getMeasurements(), measurements);
20749         assertTrue(calibrator.isCommonAxisUsed());
20750         assertSame(calibrator.getListener(), this);
20751         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20752         assertFalse(calibrator.isReady());
20753         assertFalse(calibrator.isRunning());
20754         assertNull(calibrator.getEstimatedBiases());
20755         assertFalse(calibrator.getEstimatedBiases(null));
20756         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20757         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20758         assertNull(calibrator.getEstimatedBiasFx());
20759         assertNull(calibrator.getEstimatedBiasFy());
20760         assertNull(calibrator.getEstimatedBiasFz());
20761         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20762         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20763         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20764         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20765         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20766         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20767         assertNull(calibrator.getEstimatedMa());
20768         assertNull(calibrator.getEstimatedSx());
20769         assertNull(calibrator.getEstimatedSy());
20770         assertNull(calibrator.getEstimatedSz());
20771         assertNull(calibrator.getEstimatedMxy());
20772         assertNull(calibrator.getEstimatedMxz());
20773         assertNull(calibrator.getEstimatedMyx());
20774         assertNull(calibrator.getEstimatedMyz());
20775         assertNull(calibrator.getEstimatedMzx());
20776         assertNull(calibrator.getEstimatedMzy());
20777         assertNull(calibrator.getEstimatedCovariance());
20778         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20779     }
20780 
20781     @Test
20782     public void testConstructor173() throws WrongSizeException {
20783         final Matrix ba = generateBa();
20784         final double biasX = ba.getElementAtIndex(0);
20785         final double biasY = ba.getElementAtIndex(1);
20786         final double biasZ = ba.getElementAtIndex(2);
20787 
20788         final Acceleration bx = new Acceleration(biasX,
20789                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20790         final Acceleration by = new Acceleration(biasY,
20791                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20792         final Acceleration bz = new Acceleration(biasZ,
20793                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20794 
20795         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20796         final double latitude = Math.toRadians(
20797                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20798         final double longitude = Math.toRadians(
20799                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20800         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20801         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20802         final NEDVelocity nedVelocity = new NEDVelocity();
20803         final ECEFPosition ecefPosition = new ECEFPosition();
20804         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20805         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20806                 ecefPosition, ecefVelocity);
20807 
20808         final KnownPositionAccelerometerCalibrator calibrator =
20809                 new KnownPositionAccelerometerCalibrator(nedPosition,
20810                         bx, by, bz);
20811 
20812         // check default values
20813         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20814         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20815         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20816         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20817         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20818         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20819         final Acceleration bx2 = new Acceleration(0.0,
20820                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20821         calibrator.getInitialBiasXAsAcceleration(bx2);
20822         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20823         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20824         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20825         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20826         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20827         final Acceleration by2 = new Acceleration(0.0,
20828                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20829         calibrator.getInitialBiasYAsAcceleration(by2);
20830         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20831         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20832         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20833         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20834         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20835         final Acceleration bz2 = new Acceleration(0.0,
20836                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20837         calibrator.getInitialBiasZAsAcceleration(bz2);
20838         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20839         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20840         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20841         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20842         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20843         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20844         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20845         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20846         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20847         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20848         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20849         final double[] bias1 = calibrator.getInitialBias();
20850         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20851         final double[] bias2 = new double[3];
20852         calibrator.getInitialBias(bias2);
20853         assertArrayEquals(bias1, bias2, 0.0);
20854         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20855         assertEquals(b1, ba);
20856         final Matrix b2 = new Matrix(3, 1);
20857         calibrator.getInitialBiasAsMatrix(b2);
20858         assertEquals(b1, b2);
20859         final Matrix ma1 = calibrator.getInitialMa();
20860         assertEquals(ma1, new Matrix(3, 3));
20861         final Matrix ma2 = new Matrix(3, 3);
20862         calibrator.getInitialMa(ma2);
20863         assertEquals(ma1, ma2);
20864         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20865         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20866         final NEDPosition nedPosition1 = new NEDPosition();
20867         assertTrue(calibrator.getNedPosition(nedPosition1));
20868         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20869         assertNull(calibrator.getMeasurements());
20870         assertFalse(calibrator.isCommonAxisUsed());
20871         assertNull(calibrator.getListener());
20872         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20873         assertFalse(calibrator.isReady());
20874         assertFalse(calibrator.isRunning());
20875         assertNull(calibrator.getEstimatedBiases());
20876         assertFalse(calibrator.getEstimatedBiases(null));
20877         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20878         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20879         assertNull(calibrator.getEstimatedBiasFx());
20880         assertNull(calibrator.getEstimatedBiasFy());
20881         assertNull(calibrator.getEstimatedBiasFz());
20882         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20883         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20884         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20885         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20886         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20887         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20888         assertNull(calibrator.getEstimatedMa());
20889         assertNull(calibrator.getEstimatedSx());
20890         assertNull(calibrator.getEstimatedSy());
20891         assertNull(calibrator.getEstimatedSz());
20892         assertNull(calibrator.getEstimatedMxy());
20893         assertNull(calibrator.getEstimatedMxz());
20894         assertNull(calibrator.getEstimatedMyx());
20895         assertNull(calibrator.getEstimatedMyz());
20896         assertNull(calibrator.getEstimatedMzx());
20897         assertNull(calibrator.getEstimatedMzy());
20898         assertNull(calibrator.getEstimatedCovariance());
20899         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20900     }
20901 
20902     @Test
20903     public void testConstructor174() throws WrongSizeException {
20904         final Matrix ba = generateBa();
20905         final double biasX = ba.getElementAtIndex(0);
20906         final double biasY = ba.getElementAtIndex(1);
20907         final double biasZ = ba.getElementAtIndex(2);
20908 
20909         final Acceleration bx = new Acceleration(biasX,
20910                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20911         final Acceleration by = new Acceleration(biasY,
20912                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20913         final Acceleration bz = new Acceleration(biasZ,
20914                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
20915 
20916         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20917         final double latitude = Math.toRadians(
20918                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20919         final double longitude = Math.toRadians(
20920                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20921         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20922         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20923         final NEDVelocity nedVelocity = new NEDVelocity();
20924         final ECEFPosition ecefPosition = new ECEFPosition();
20925         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20926         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20927                 ecefPosition, ecefVelocity);
20928 
20929         final KnownPositionAccelerometerCalibrator calibrator =
20930                 new KnownPositionAccelerometerCalibrator(nedPosition,
20931                         bx, by, bz, this);
20932 
20933         // check default values
20934         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20935         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20936         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20937         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20938         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20939         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20940         final Acceleration bx2 = new Acceleration(0.0,
20941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20942         calibrator.getInitialBiasXAsAcceleration(bx2);
20943         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20944         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20945         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20946         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20947         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20948         final Acceleration by2 = new Acceleration(0.0,
20949                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20950         calibrator.getInitialBiasYAsAcceleration(by2);
20951         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20952         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20953         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20954         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20955         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20956         final Acceleration bz2 = new Acceleration(0.0,
20957                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20958         calibrator.getInitialBiasZAsAcceleration(bz2);
20959         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20960         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20961         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20962         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20963         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20964         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20965         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20966         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20967         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20968         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20969         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20970         final double[] bias1 = calibrator.getInitialBias();
20971         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
20972         final double[] bias2 = new double[3];
20973         calibrator.getInitialBias(bias2);
20974         assertArrayEquals(bias1, bias2, 0.0);
20975         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20976         assertEquals(b1, ba);
20977         final Matrix b2 = new Matrix(3, 1);
20978         calibrator.getInitialBiasAsMatrix(b2);
20979         assertEquals(b1, b2);
20980         final Matrix ma1 = calibrator.getInitialMa();
20981         assertEquals(ma1, new Matrix(3, 3));
20982         final Matrix ma2 = new Matrix(3, 3);
20983         calibrator.getInitialMa(ma2);
20984         assertEquals(ma1, ma2);
20985         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20986         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20987         final NEDPosition nedPosition1 = new NEDPosition();
20988         assertTrue(calibrator.getNedPosition(nedPosition1));
20989         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20990         assertNull(calibrator.getMeasurements());
20991         assertFalse(calibrator.isCommonAxisUsed());
20992         assertSame(calibrator.getListener(), this);
20993         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20994         assertFalse(calibrator.isReady());
20995         assertFalse(calibrator.isRunning());
20996         assertNull(calibrator.getEstimatedBiases());
20997         assertFalse(calibrator.getEstimatedBiases(null));
20998         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20999         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21000         assertNull(calibrator.getEstimatedBiasFx());
21001         assertNull(calibrator.getEstimatedBiasFy());
21002         assertNull(calibrator.getEstimatedBiasFz());
21003         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21004         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21005         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21006         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21007         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21008         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21009         assertNull(calibrator.getEstimatedMa());
21010         assertNull(calibrator.getEstimatedSx());
21011         assertNull(calibrator.getEstimatedSy());
21012         assertNull(calibrator.getEstimatedSz());
21013         assertNull(calibrator.getEstimatedMxy());
21014         assertNull(calibrator.getEstimatedMxz());
21015         assertNull(calibrator.getEstimatedMyx());
21016         assertNull(calibrator.getEstimatedMyz());
21017         assertNull(calibrator.getEstimatedMzx());
21018         assertNull(calibrator.getEstimatedMzy());
21019         assertNull(calibrator.getEstimatedCovariance());
21020         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21021     }
21022 
21023     @Test
21024     public void testConstructor175() throws WrongSizeException {
21025         final Collection<StandardDeviationBodyKinematics> measurements =
21026                 Collections.emptyList();
21027 
21028         final Matrix ba = generateBa();
21029         final double biasX = ba.getElementAtIndex(0);
21030         final double biasY = ba.getElementAtIndex(1);
21031         final double biasZ = ba.getElementAtIndex(2);
21032 
21033         final Acceleration bx = new Acceleration(biasX,
21034                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21035         final Acceleration by = new Acceleration(biasY,
21036                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21037         final Acceleration bz = new Acceleration(biasZ,
21038                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21039 
21040         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21041         final double latitude = Math.toRadians(
21042                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21043         final double longitude = Math.toRadians(
21044                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21045         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21046         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21047         final NEDVelocity nedVelocity = new NEDVelocity();
21048         final ECEFPosition ecefPosition = new ECEFPosition();
21049         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21050         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21051                 ecefPosition, ecefVelocity);
21052 
21053         final KnownPositionAccelerometerCalibrator calibrator =
21054                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21055                         bx, by, bz);
21056 
21057         // check default values
21058         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21059         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21060         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21061         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21062         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21063         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21064         final Acceleration bx2 = new Acceleration(0.0,
21065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21066         calibrator.getInitialBiasXAsAcceleration(bx2);
21067         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21068         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21069         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21070         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21071         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21072         final Acceleration by2 = new Acceleration(0.0,
21073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21074         calibrator.getInitialBiasYAsAcceleration(by2);
21075         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21076         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21077         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21078         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21079         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21080         final Acceleration bz2 = new Acceleration(0.0,
21081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21082         calibrator.getInitialBiasZAsAcceleration(bz2);
21083         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21084         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21085         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21086         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21087         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21088         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21089         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21090         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21091         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21092         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21093         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21094         final double[] bias1 = calibrator.getInitialBias();
21095         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21096         final double[] bias2 = new double[3];
21097         calibrator.getInitialBias(bias2);
21098         assertArrayEquals(bias1, bias2, 0.0);
21099         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21100         assertEquals(b1, ba);
21101         final Matrix b2 = new Matrix(3, 1);
21102         calibrator.getInitialBiasAsMatrix(b2);
21103         assertEquals(b1, b2);
21104         final Matrix ma1 = calibrator.getInitialMa();
21105         assertEquals(ma1, new Matrix(3, 3));
21106         final Matrix ma2 = new Matrix(3, 3);
21107         calibrator.getInitialMa(ma2);
21108         assertEquals(ma1, ma2);
21109         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21110         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21111         final NEDPosition nedPosition1 = new NEDPosition();
21112         assertTrue(calibrator.getNedPosition(nedPosition1));
21113         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21114         assertSame(calibrator.getMeasurements(), measurements);
21115         assertFalse(calibrator.isCommonAxisUsed());
21116         assertNull(calibrator.getListener());
21117         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21118         assertFalse(calibrator.isReady());
21119         assertFalse(calibrator.isRunning());
21120         assertNull(calibrator.getEstimatedBiases());
21121         assertFalse(calibrator.getEstimatedBiases(null));
21122         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21123         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21124         assertNull(calibrator.getEstimatedBiasFx());
21125         assertNull(calibrator.getEstimatedBiasFy());
21126         assertNull(calibrator.getEstimatedBiasFz());
21127         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21128         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21129         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21130         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21131         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21132         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21133         assertNull(calibrator.getEstimatedMa());
21134         assertNull(calibrator.getEstimatedSx());
21135         assertNull(calibrator.getEstimatedSy());
21136         assertNull(calibrator.getEstimatedSz());
21137         assertNull(calibrator.getEstimatedMxy());
21138         assertNull(calibrator.getEstimatedMxz());
21139         assertNull(calibrator.getEstimatedMyx());
21140         assertNull(calibrator.getEstimatedMyz());
21141         assertNull(calibrator.getEstimatedMzx());
21142         assertNull(calibrator.getEstimatedMzy());
21143         assertNull(calibrator.getEstimatedCovariance());
21144         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21145     }
21146 
21147     @Test
21148     public void testConstructor176() throws WrongSizeException {
21149         final Collection<StandardDeviationBodyKinematics> measurements =
21150                 Collections.emptyList();
21151 
21152         final Matrix ba = generateBa();
21153         final double biasX = ba.getElementAtIndex(0);
21154         final double biasY = ba.getElementAtIndex(1);
21155         final double biasZ = ba.getElementAtIndex(2);
21156 
21157         final Acceleration bx = new Acceleration(biasX,
21158                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21159         final Acceleration by = new Acceleration(biasY,
21160                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21161         final Acceleration bz = new Acceleration(biasZ,
21162                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21163 
21164         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21165         final double latitude = Math.toRadians(
21166                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21167         final double longitude = Math.toRadians(
21168                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21169         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21170         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21171         final NEDVelocity nedVelocity = new NEDVelocity();
21172         final ECEFPosition ecefPosition = new ECEFPosition();
21173         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21174         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21175                 ecefPosition, ecefVelocity);
21176 
21177         final KnownPositionAccelerometerCalibrator calibrator =
21178                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21179                         bx, by, bz, this);
21180 
21181         // check default values
21182         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21183         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21184         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21185         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21186         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21187         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21188         final Acceleration bx2 = new Acceleration(0.0,
21189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21190         calibrator.getInitialBiasXAsAcceleration(bx2);
21191         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21192         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21193         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21194         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21195         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21196         final Acceleration by2 = new Acceleration(0.0,
21197                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21198         calibrator.getInitialBiasYAsAcceleration(by2);
21199         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21200         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21201         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21202         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21203         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21204         final Acceleration bz2 = new Acceleration(0.0,
21205                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21206         calibrator.getInitialBiasZAsAcceleration(bz2);
21207         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21208         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21209         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21210         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21211         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21212         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21213         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21214         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21215         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21216         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21217         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21218         final double[] bias1 = calibrator.getInitialBias();
21219         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21220         final double[] bias2 = new double[3];
21221         calibrator.getInitialBias(bias2);
21222         assertArrayEquals(bias1, bias2, 0.0);
21223         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21224         assertEquals(b1, ba);
21225         final Matrix b2 = new Matrix(3, 1);
21226         calibrator.getInitialBiasAsMatrix(b2);
21227         assertEquals(b1, b2);
21228         final Matrix ma1 = calibrator.getInitialMa();
21229         assertEquals(ma1, new Matrix(3, 3));
21230         final Matrix ma2 = new Matrix(3, 3);
21231         calibrator.getInitialMa(ma2);
21232         assertEquals(ma1, ma2);
21233         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21234         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21235         final NEDPosition nedPosition1 = new NEDPosition();
21236         assertTrue(calibrator.getNedPosition(nedPosition1));
21237         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21238         assertSame(calibrator.getMeasurements(), measurements);
21239         assertFalse(calibrator.isCommonAxisUsed());
21240         assertSame(calibrator.getListener(), this);
21241         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21242         assertFalse(calibrator.isReady());
21243         assertFalse(calibrator.isRunning());
21244         assertNull(calibrator.getEstimatedBiases());
21245         assertFalse(calibrator.getEstimatedBiases(null));
21246         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21247         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21248         assertNull(calibrator.getEstimatedBiasFx());
21249         assertNull(calibrator.getEstimatedBiasFy());
21250         assertNull(calibrator.getEstimatedBiasFz());
21251         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21252         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21253         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21254         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21255         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21256         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21257         assertNull(calibrator.getEstimatedMa());
21258         assertNull(calibrator.getEstimatedSx());
21259         assertNull(calibrator.getEstimatedSy());
21260         assertNull(calibrator.getEstimatedSz());
21261         assertNull(calibrator.getEstimatedMxy());
21262         assertNull(calibrator.getEstimatedMxz());
21263         assertNull(calibrator.getEstimatedMyx());
21264         assertNull(calibrator.getEstimatedMyz());
21265         assertNull(calibrator.getEstimatedMzx());
21266         assertNull(calibrator.getEstimatedMzy());
21267         assertNull(calibrator.getEstimatedCovariance());
21268         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21269     }
21270 
21271     @Test
21272     public void testConstructor177() throws WrongSizeException {
21273         final Matrix ba = generateBa();
21274         final double biasX = ba.getElementAtIndex(0);
21275         final double biasY = ba.getElementAtIndex(1);
21276         final double biasZ = ba.getElementAtIndex(2);
21277 
21278         final Acceleration bx = new Acceleration(biasX,
21279                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21280         final Acceleration by = new Acceleration(biasY,
21281                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21282         final Acceleration bz = new Acceleration(biasZ,
21283                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21284 
21285         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21286         final double latitude = Math.toRadians(
21287                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21288         final double longitude = Math.toRadians(
21289                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21290         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21291         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21292         final NEDVelocity nedVelocity = new NEDVelocity();
21293         final ECEFPosition ecefPosition = new ECEFPosition();
21294         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21295         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21296                 ecefPosition, ecefVelocity);
21297 
21298         final KnownPositionAccelerometerCalibrator calibrator =
21299                 new KnownPositionAccelerometerCalibrator(nedPosition,
21300                         true, bx, by, bz);
21301 
21302         // check default values
21303         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21304         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21305         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21306         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21307         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21308         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21309         final Acceleration bx2 = new Acceleration(0.0,
21310                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21311         calibrator.getInitialBiasXAsAcceleration(bx2);
21312         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21313         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21314         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21315         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21316         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21317         final Acceleration by2 = new Acceleration(0.0,
21318                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21319         calibrator.getInitialBiasYAsAcceleration(by2);
21320         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21321         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21322         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21323         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21324         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21325         final Acceleration bz2 = new Acceleration(0.0,
21326                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21327         calibrator.getInitialBiasZAsAcceleration(bz2);
21328         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21329         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21330         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21331         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21332         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21333         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21334         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21335         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21336         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21337         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21338         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21339         final double[] bias1 = calibrator.getInitialBias();
21340         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21341         final double[] bias2 = new double[3];
21342         calibrator.getInitialBias(bias2);
21343         assertArrayEquals(bias1, bias2, 0.0);
21344         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21345         assertEquals(b1, ba);
21346         final Matrix b2 = new Matrix(3, 1);
21347         calibrator.getInitialBiasAsMatrix(b2);
21348         assertEquals(b1, b2);
21349         final Matrix ma1 = calibrator.getInitialMa();
21350         assertEquals(ma1, new Matrix(3, 3));
21351         final Matrix ma2 = new Matrix(3, 3);
21352         calibrator.getInitialMa(ma2);
21353         assertEquals(ma1, ma2);
21354         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21355         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21356         final NEDPosition nedPosition1 = new NEDPosition();
21357         assertTrue(calibrator.getNedPosition(nedPosition1));
21358         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21359         assertNull(calibrator.getMeasurements());
21360         assertTrue(calibrator.isCommonAxisUsed());
21361         assertNull(calibrator.getListener());
21362         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21363         assertFalse(calibrator.isReady());
21364         assertFalse(calibrator.isRunning());
21365         assertNull(calibrator.getEstimatedBiases());
21366         assertFalse(calibrator.getEstimatedBiases(null));
21367         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21368         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21369         assertNull(calibrator.getEstimatedBiasFx());
21370         assertNull(calibrator.getEstimatedBiasFy());
21371         assertNull(calibrator.getEstimatedBiasFz());
21372         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21373         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21374         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21375         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21376         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21377         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21378         assertNull(calibrator.getEstimatedMa());
21379         assertNull(calibrator.getEstimatedSx());
21380         assertNull(calibrator.getEstimatedSy());
21381         assertNull(calibrator.getEstimatedSz());
21382         assertNull(calibrator.getEstimatedMxy());
21383         assertNull(calibrator.getEstimatedMxz());
21384         assertNull(calibrator.getEstimatedMyx());
21385         assertNull(calibrator.getEstimatedMyz());
21386         assertNull(calibrator.getEstimatedMzx());
21387         assertNull(calibrator.getEstimatedMzy());
21388         assertNull(calibrator.getEstimatedCovariance());
21389         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21390     }
21391 
21392     @Test
21393     public void testConstructor178() throws WrongSizeException {
21394         final Matrix ba = generateBa();
21395         final double biasX = ba.getElementAtIndex(0);
21396         final double biasY = ba.getElementAtIndex(1);
21397         final double biasZ = ba.getElementAtIndex(2);
21398 
21399         final Acceleration bx = new Acceleration(biasX,
21400                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21401         final Acceleration by = new Acceleration(biasY,
21402                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21403         final Acceleration bz = new Acceleration(biasZ,
21404                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21405 
21406         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21407         final double latitude = Math.toRadians(
21408                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21409         final double longitude = Math.toRadians(
21410                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21411         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21412         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21413         final NEDVelocity nedVelocity = new NEDVelocity();
21414         final ECEFPosition ecefPosition = new ECEFPosition();
21415         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21416         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21417                 ecefPosition, ecefVelocity);
21418 
21419         final KnownPositionAccelerometerCalibrator calibrator =
21420                 new KnownPositionAccelerometerCalibrator(nedPosition,
21421                         true, bx, by, bz, this);
21422 
21423         // check default values
21424         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21425         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21426         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21427         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21428         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21429         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21430         final Acceleration bx2 = new Acceleration(0.0,
21431                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21432         calibrator.getInitialBiasXAsAcceleration(bx2);
21433         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21434         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21435         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21436         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21437         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21438         final Acceleration by2 = new Acceleration(0.0,
21439                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21440         calibrator.getInitialBiasYAsAcceleration(by2);
21441         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21442         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21443         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21444         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21445         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21446         final Acceleration bz2 = new Acceleration(0.0,
21447                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21448         calibrator.getInitialBiasZAsAcceleration(bz2);
21449         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21450         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21451         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21452         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21453         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21454         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21455         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21456         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21457         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21458         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21459         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21460         final double[] bias1 = calibrator.getInitialBias();
21461         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21462         final double[] bias2 = new double[3];
21463         calibrator.getInitialBias(bias2);
21464         assertArrayEquals(bias1, bias2, 0.0);
21465         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21466         assertEquals(b1, ba);
21467         final Matrix b2 = new Matrix(3, 1);
21468         calibrator.getInitialBiasAsMatrix(b2);
21469         assertEquals(b1, b2);
21470         final Matrix ma1 = calibrator.getInitialMa();
21471         assertEquals(ma1, new Matrix(3, 3));
21472         final Matrix ma2 = new Matrix(3, 3);
21473         calibrator.getInitialMa(ma2);
21474         assertEquals(ma1, ma2);
21475         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21476         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21477         final NEDPosition nedPosition1 = new NEDPosition();
21478         assertTrue(calibrator.getNedPosition(nedPosition1));
21479         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21480         assertNull(calibrator.getMeasurements());
21481         assertTrue(calibrator.isCommonAxisUsed());
21482         assertSame(calibrator.getListener(), this);
21483         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21484         assertFalse(calibrator.isReady());
21485         assertFalse(calibrator.isRunning());
21486         assertNull(calibrator.getEstimatedBiases());
21487         assertFalse(calibrator.getEstimatedBiases(null));
21488         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21489         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21490         assertNull(calibrator.getEstimatedBiasFx());
21491         assertNull(calibrator.getEstimatedBiasFy());
21492         assertNull(calibrator.getEstimatedBiasFz());
21493         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21494         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21495         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21496         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21497         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21498         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21499         assertNull(calibrator.getEstimatedMa());
21500         assertNull(calibrator.getEstimatedSx());
21501         assertNull(calibrator.getEstimatedSy());
21502         assertNull(calibrator.getEstimatedSz());
21503         assertNull(calibrator.getEstimatedMxy());
21504         assertNull(calibrator.getEstimatedMxz());
21505         assertNull(calibrator.getEstimatedMyx());
21506         assertNull(calibrator.getEstimatedMyz());
21507         assertNull(calibrator.getEstimatedMzx());
21508         assertNull(calibrator.getEstimatedMzy());
21509         assertNull(calibrator.getEstimatedCovariance());
21510         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21511     }
21512 
21513     @Test
21514     public void testConstructor179() throws WrongSizeException {
21515         final Collection<StandardDeviationBodyKinematics> measurements =
21516                 Collections.emptyList();
21517 
21518         final Matrix ba = generateBa();
21519         final double biasX = ba.getElementAtIndex(0);
21520         final double biasY = ba.getElementAtIndex(1);
21521         final double biasZ = ba.getElementAtIndex(2);
21522 
21523         final Acceleration bx = new Acceleration(biasX,
21524                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21525         final Acceleration by = new Acceleration(biasY,
21526                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21527         final Acceleration bz = new Acceleration(biasZ,
21528                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21529 
21530         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21531         final double latitude = Math.toRadians(
21532                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21533         final double longitude = Math.toRadians(
21534                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21535         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21536         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21537         final NEDVelocity nedVelocity = new NEDVelocity();
21538         final ECEFPosition ecefPosition = new ECEFPosition();
21539         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21540         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21541                 ecefPosition, ecefVelocity);
21542 
21543         final KnownPositionAccelerometerCalibrator calibrator =
21544                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21545                         true, bx, by, bz);
21546 
21547         // check default values
21548         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21549         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21550         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21551         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21552         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21553         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21554         final Acceleration bx2 = new Acceleration(0.0,
21555                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21556         calibrator.getInitialBiasXAsAcceleration(bx2);
21557         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21558         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21559         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21560         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21561         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21562         final Acceleration by2 = new Acceleration(0.0,
21563                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21564         calibrator.getInitialBiasYAsAcceleration(by2);
21565         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21566         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21567         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21568         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21569         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21570         final Acceleration bz2 = new Acceleration(0.0,
21571                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21572         calibrator.getInitialBiasZAsAcceleration(bz2);
21573         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21574         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21575         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21576         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21577         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21578         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21579         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21580         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21581         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21582         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21583         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21584         final double[] bias1 = calibrator.getInitialBias();
21585         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21586         final double[] bias2 = new double[3];
21587         calibrator.getInitialBias(bias2);
21588         assertArrayEquals(bias1, bias2, 0.0);
21589         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21590         assertEquals(b1, ba);
21591         final Matrix b2 = new Matrix(3, 1);
21592         calibrator.getInitialBiasAsMatrix(b2);
21593         assertEquals(b1, b2);
21594         final Matrix ma1 = calibrator.getInitialMa();
21595         assertEquals(ma1, new Matrix(3, 3));
21596         final Matrix ma2 = new Matrix(3, 3);
21597         calibrator.getInitialMa(ma2);
21598         assertEquals(ma1, ma2);
21599         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21600         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21601         final NEDPosition nedPosition1 = new NEDPosition();
21602         assertTrue(calibrator.getNedPosition(nedPosition1));
21603         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21604         assertSame(calibrator.getMeasurements(), measurements);
21605         assertTrue(calibrator.isCommonAxisUsed());
21606         assertNull(calibrator.getListener());
21607         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21608         assertFalse(calibrator.isReady());
21609         assertFalse(calibrator.isRunning());
21610         assertNull(calibrator.getEstimatedBiases());
21611         assertFalse(calibrator.getEstimatedBiases(null));
21612         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21613         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21614         assertNull(calibrator.getEstimatedBiasFx());
21615         assertNull(calibrator.getEstimatedBiasFy());
21616         assertNull(calibrator.getEstimatedBiasFz());
21617         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21618         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21619         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21620         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21621         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21622         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21623         assertNull(calibrator.getEstimatedMa());
21624         assertNull(calibrator.getEstimatedSx());
21625         assertNull(calibrator.getEstimatedSy());
21626         assertNull(calibrator.getEstimatedSz());
21627         assertNull(calibrator.getEstimatedMxy());
21628         assertNull(calibrator.getEstimatedMxz());
21629         assertNull(calibrator.getEstimatedMyx());
21630         assertNull(calibrator.getEstimatedMyz());
21631         assertNull(calibrator.getEstimatedMzx());
21632         assertNull(calibrator.getEstimatedMzy());
21633         assertNull(calibrator.getEstimatedCovariance());
21634         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21635     }
21636 
21637     @Test
21638     public void testConstructor180() throws WrongSizeException {
21639         final Collection<StandardDeviationBodyKinematics> measurements =
21640                 Collections.emptyList();
21641 
21642         final Matrix ba = generateBa();
21643         final double biasX = ba.getElementAtIndex(0);
21644         final double biasY = ba.getElementAtIndex(1);
21645         final double biasZ = ba.getElementAtIndex(2);
21646 
21647         final Acceleration bx = new Acceleration(biasX,
21648                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21649         final Acceleration by = new Acceleration(biasY,
21650                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21651         final Acceleration bz = new Acceleration(biasZ,
21652                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21653 
21654         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21655         final double latitude = Math.toRadians(
21656                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21657         final double longitude = Math.toRadians(
21658                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21659         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21660         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21661         final NEDVelocity nedVelocity = new NEDVelocity();
21662         final ECEFPosition ecefPosition = new ECEFPosition();
21663         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21664         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21665                 ecefPosition, ecefVelocity);
21666 
21667         final KnownPositionAccelerometerCalibrator calibrator =
21668                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21669                         true, bx, by, bz, this);
21670 
21671         // check default values
21672         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21673         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21674         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21675         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21676         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21677         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21678         final Acceleration bx2 = new Acceleration(0.0,
21679                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21680         calibrator.getInitialBiasXAsAcceleration(bx2);
21681         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21682         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21683         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21684         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21685         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21686         final Acceleration by2 = new Acceleration(0.0,
21687                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21688         calibrator.getInitialBiasYAsAcceleration(by2);
21689         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21690         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21691         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21692         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21693         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21694         final Acceleration bz2 = new Acceleration(0.0,
21695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21696         calibrator.getInitialBiasZAsAcceleration(bz2);
21697         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21698         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21699         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21700         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21701         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21702         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21703         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21704         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21705         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21706         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21707         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21708         final double[] bias1 = calibrator.getInitialBias();
21709         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21710         final double[] bias2 = new double[3];
21711         calibrator.getInitialBias(bias2);
21712         assertArrayEquals(bias1, bias2, 0.0);
21713         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21714         assertEquals(b1, ba);
21715         final Matrix b2 = new Matrix(3, 1);
21716         calibrator.getInitialBiasAsMatrix(b2);
21717         assertEquals(b1, b2);
21718         final Matrix ma1 = calibrator.getInitialMa();
21719         assertEquals(ma1, new Matrix(3, 3));
21720         final Matrix ma2 = new Matrix(3, 3);
21721         calibrator.getInitialMa(ma2);
21722         assertEquals(ma1, ma2);
21723         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21724         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21725         final NEDPosition nedPosition1 = new NEDPosition();
21726         assertTrue(calibrator.getNedPosition(nedPosition1));
21727         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21728         assertSame(calibrator.getMeasurements(), measurements);
21729         assertTrue(calibrator.isCommonAxisUsed());
21730         assertSame(calibrator.getListener(), this);
21731         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21732         assertFalse(calibrator.isReady());
21733         assertFalse(calibrator.isRunning());
21734         assertNull(calibrator.getEstimatedBiases());
21735         assertFalse(calibrator.getEstimatedBiases(null));
21736         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21737         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21738         assertNull(calibrator.getEstimatedBiasFx());
21739         assertNull(calibrator.getEstimatedBiasFy());
21740         assertNull(calibrator.getEstimatedBiasFz());
21741         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21742         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21743         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21744         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21745         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21746         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21747         assertNull(calibrator.getEstimatedMa());
21748         assertNull(calibrator.getEstimatedSx());
21749         assertNull(calibrator.getEstimatedSy());
21750         assertNull(calibrator.getEstimatedSz());
21751         assertNull(calibrator.getEstimatedMxy());
21752         assertNull(calibrator.getEstimatedMxz());
21753         assertNull(calibrator.getEstimatedMyx());
21754         assertNull(calibrator.getEstimatedMyz());
21755         assertNull(calibrator.getEstimatedMzx());
21756         assertNull(calibrator.getEstimatedMzy());
21757         assertNull(calibrator.getEstimatedCovariance());
21758         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21759     }
21760 
21761     @Test
21762     public void testConstructor181() throws WrongSizeException {
21763         final Matrix ba = generateBa();
21764         final double biasX = ba.getElementAtIndex(0);
21765         final double biasY = ba.getElementAtIndex(1);
21766         final double biasZ = ba.getElementAtIndex(2);
21767 
21768         final Matrix ma = generateMaCommonAxis();
21769         final double sx = ma.getElementAt(0, 0);
21770         final double sy = ma.getElementAt(1, 1);
21771         final double sz = ma.getElementAt(2, 2);
21772 
21773         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21774         final double latitude = Math.toRadians(
21775                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21776         final double longitude = Math.toRadians(
21777                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21778         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21779         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21780         final NEDVelocity nedVelocity = new NEDVelocity();
21781         final ECEFPosition ecefPosition = new ECEFPosition();
21782         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21783         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21784                 ecefPosition, ecefVelocity);
21785 
21786         final KnownPositionAccelerometerCalibrator calibrator =
21787                 new KnownPositionAccelerometerCalibrator(nedPosition,
21788                         biasX, biasY, biasZ, sx, sy, sz);
21789 
21790         // check default values
21791         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21792         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21793         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21794         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21795         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21796         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21797         final Acceleration bx2 = new Acceleration(0.0,
21798                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21799         calibrator.getInitialBiasXAsAcceleration(bx2);
21800         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21801         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21802         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21803         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21804         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21805         final Acceleration by2 = new Acceleration(0.0,
21806                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21807         calibrator.getInitialBiasYAsAcceleration(by2);
21808         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21809         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21810         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21811         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21812         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21813         final Acceleration bz2 = new Acceleration(0.0,
21814                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21815         calibrator.getInitialBiasZAsAcceleration(bz2);
21816         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21817         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21818         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21819         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21820         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21821         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21822         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21823         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21824         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21825         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21826         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21827         final double[] bias1 = calibrator.getInitialBias();
21828         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21829         final double[] bias2 = new double[3];
21830         calibrator.getInitialBias(bias2);
21831         assertArrayEquals(bias1, bias2, 0.0);
21832         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21833         assertEquals(b1, ba);
21834         final Matrix b2 = new Matrix(3, 1);
21835         calibrator.getInitialBiasAsMatrix(b2);
21836         assertEquals(b1, b2);
21837         final Matrix ma1 = calibrator.getInitialMa();
21838         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
21839         final Matrix ma2 = new Matrix(3, 3);
21840         calibrator.getInitialMa(ma2);
21841         assertEquals(ma1, ma2);
21842         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21843         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21844         final NEDPosition nedPosition1 = new NEDPosition();
21845         assertTrue(calibrator.getNedPosition(nedPosition1));
21846         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21847         assertNull(calibrator.getMeasurements());
21848         assertFalse(calibrator.isCommonAxisUsed());
21849         assertNull(calibrator.getListener());
21850         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21851         assertFalse(calibrator.isReady());
21852         assertFalse(calibrator.isRunning());
21853         assertNull(calibrator.getEstimatedBiases());
21854         assertFalse(calibrator.getEstimatedBiases(null));
21855         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21856         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21857         assertNull(calibrator.getEstimatedBiasFx());
21858         assertNull(calibrator.getEstimatedBiasFy());
21859         assertNull(calibrator.getEstimatedBiasFz());
21860         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21861         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21862         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21863         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21864         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21865         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21866         assertNull(calibrator.getEstimatedMa());
21867         assertNull(calibrator.getEstimatedSx());
21868         assertNull(calibrator.getEstimatedSy());
21869         assertNull(calibrator.getEstimatedSz());
21870         assertNull(calibrator.getEstimatedMxy());
21871         assertNull(calibrator.getEstimatedMxz());
21872         assertNull(calibrator.getEstimatedMyx());
21873         assertNull(calibrator.getEstimatedMyz());
21874         assertNull(calibrator.getEstimatedMzx());
21875         assertNull(calibrator.getEstimatedMzy());
21876         assertNull(calibrator.getEstimatedCovariance());
21877         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21878     }
21879 
21880     @Test
21881     public void testConstructor182() throws WrongSizeException {
21882         final Collection<StandardDeviationBodyKinematics> measurements =
21883                 Collections.emptyList();
21884 
21885         final Matrix ba = generateBa();
21886         final double biasX = ba.getElementAtIndex(0);
21887         final double biasY = ba.getElementAtIndex(1);
21888         final double biasZ = ba.getElementAtIndex(2);
21889 
21890         final Matrix ma = generateMaCommonAxis();
21891         final double sx = ma.getElementAt(0, 0);
21892         final double sy = ma.getElementAt(1, 1);
21893         final double sz = ma.getElementAt(2, 2);
21894 
21895         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21896         final double latitude = Math.toRadians(
21897                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21898         final double longitude = Math.toRadians(
21899                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21900         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21901         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21902         final NEDVelocity nedVelocity = new NEDVelocity();
21903         final ECEFPosition ecefPosition = new ECEFPosition();
21904         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21905         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21906                 ecefPosition, ecefVelocity);
21907 
21908         final KnownPositionAccelerometerCalibrator calibrator =
21909                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21910                         biasX, biasY, biasZ, sx, sy, sz);
21911 
21912         // check default values
21913         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21914         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21915         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21916         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21917         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21918         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21919         final Acceleration bx2 = new Acceleration(0.0,
21920                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21921         calibrator.getInitialBiasXAsAcceleration(bx2);
21922         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21923         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21924         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21925         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21926         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21927         final Acceleration by2 = new Acceleration(0.0,
21928                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21929         calibrator.getInitialBiasYAsAcceleration(by2);
21930         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21931         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21932         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21933         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21934         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21935         final Acceleration bz2 = new Acceleration(0.0,
21936                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21937         calibrator.getInitialBiasZAsAcceleration(bz2);
21938         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21939         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21940         assertEquals(calibrator.getInitialSx(), sx, 0.0);
21941         assertEquals(calibrator.getInitialSy(), sy, 0.0);
21942         assertEquals(calibrator.getInitialSz(), sz, 0.0);
21943         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21944         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21945         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21946         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21947         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21948         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21949         final double[] bias1 = calibrator.getInitialBias();
21950         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
21951         final double[] bias2 = new double[3];
21952         calibrator.getInitialBias(bias2);
21953         assertArrayEquals(bias1, bias2, 0.0);
21954         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21955         assertEquals(b1, ba);
21956         final Matrix b2 = new Matrix(3, 1);
21957         calibrator.getInitialBiasAsMatrix(b2);
21958         assertEquals(b1, b2);
21959         final Matrix ma1 = calibrator.getInitialMa();
21960         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
21961         final Matrix ma2 = new Matrix(3, 3);
21962         calibrator.getInitialMa(ma2);
21963         assertEquals(ma1, ma2);
21964         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21965         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21966         final NEDPosition nedPosition1 = new NEDPosition();
21967         assertTrue(calibrator.getNedPosition(nedPosition1));
21968         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21969         assertSame(calibrator.getMeasurements(), measurements);
21970         assertFalse(calibrator.isCommonAxisUsed());
21971         assertNull(calibrator.getListener());
21972         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21973         assertFalse(calibrator.isReady());
21974         assertFalse(calibrator.isRunning());
21975         assertNull(calibrator.getEstimatedBiases());
21976         assertFalse(calibrator.getEstimatedBiases(null));
21977         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21978         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21979         assertNull(calibrator.getEstimatedBiasFx());
21980         assertNull(calibrator.getEstimatedBiasFy());
21981         assertNull(calibrator.getEstimatedBiasFz());
21982         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21983         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21984         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21985         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21986         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21987         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21988         assertNull(calibrator.getEstimatedMa());
21989         assertNull(calibrator.getEstimatedSx());
21990         assertNull(calibrator.getEstimatedSy());
21991         assertNull(calibrator.getEstimatedSz());
21992         assertNull(calibrator.getEstimatedMxy());
21993         assertNull(calibrator.getEstimatedMxz());
21994         assertNull(calibrator.getEstimatedMyx());
21995         assertNull(calibrator.getEstimatedMyz());
21996         assertNull(calibrator.getEstimatedMzx());
21997         assertNull(calibrator.getEstimatedMzy());
21998         assertNull(calibrator.getEstimatedCovariance());
21999         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22000     }
22001 
22002     @Test
22003     public void testConstructor183() throws WrongSizeException {
22004         final Collection<StandardDeviationBodyKinematics> measurements =
22005                 Collections.emptyList();
22006 
22007         final Matrix ba = generateBa();
22008         final double biasX = ba.getElementAtIndex(0);
22009         final double biasY = ba.getElementAtIndex(1);
22010         final double biasZ = ba.getElementAtIndex(2);
22011 
22012         final Matrix ma = generateMaCommonAxis();
22013         final double sx = ma.getElementAt(0, 0);
22014         final double sy = ma.getElementAt(1, 1);
22015         final double sz = ma.getElementAt(2, 2);
22016 
22017         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22018         final double latitude = Math.toRadians(
22019                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22020         final double longitude = Math.toRadians(
22021                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22022         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22023         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22024         final NEDVelocity nedVelocity = new NEDVelocity();
22025         final ECEFPosition ecefPosition = new ECEFPosition();
22026         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22027         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22028                 ecefPosition, ecefVelocity);
22029 
22030         final KnownPositionAccelerometerCalibrator calibrator =
22031                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22032                         biasX, biasY, biasZ, sx, sy, sz, this);
22033 
22034         // check default values
22035         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22036         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22037         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22038         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22039         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22040         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22041         final Acceleration bx2 = new Acceleration(0.0,
22042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22043         calibrator.getInitialBiasXAsAcceleration(bx2);
22044         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22045         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22047         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22048         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22049         final Acceleration by2 = new Acceleration(0.0,
22050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22051         calibrator.getInitialBiasYAsAcceleration(by2);
22052         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22053         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22054         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22055         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22056         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22057         final Acceleration bz2 = new Acceleration(0.0,
22058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22059         calibrator.getInitialBiasZAsAcceleration(bz2);
22060         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22061         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22062         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22063         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22064         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22065         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22066         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22067         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22068         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22069         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22070         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22071         final double[] bias1 = calibrator.getInitialBias();
22072         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22073         final double[] bias2 = new double[3];
22074         calibrator.getInitialBias(bias2);
22075         assertArrayEquals(bias1, bias2, 0.0);
22076         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22077         assertEquals(b1, ba);
22078         final Matrix b2 = new Matrix(3, 1);
22079         calibrator.getInitialBiasAsMatrix(b2);
22080         assertEquals(b1, b2);
22081         final Matrix ma1 = calibrator.getInitialMa();
22082         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22083         final Matrix ma2 = new Matrix(3, 3);
22084         calibrator.getInitialMa(ma2);
22085         assertEquals(ma1, ma2);
22086         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22087         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22088         final NEDPosition nedPosition1 = new NEDPosition();
22089         assertTrue(calibrator.getNedPosition(nedPosition1));
22090         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22091         assertSame(calibrator.getMeasurements(), measurements);
22092         assertFalse(calibrator.isCommonAxisUsed());
22093         assertSame(calibrator.getListener(), this);
22094         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22095         assertFalse(calibrator.isReady());
22096         assertFalse(calibrator.isRunning());
22097         assertNull(calibrator.getEstimatedBiases());
22098         assertFalse(calibrator.getEstimatedBiases(null));
22099         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22100         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22101         assertNull(calibrator.getEstimatedBiasFx());
22102         assertNull(calibrator.getEstimatedBiasFy());
22103         assertNull(calibrator.getEstimatedBiasFz());
22104         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22105         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22106         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22107         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22108         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22109         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22110         assertNull(calibrator.getEstimatedMa());
22111         assertNull(calibrator.getEstimatedSx());
22112         assertNull(calibrator.getEstimatedSy());
22113         assertNull(calibrator.getEstimatedSz());
22114         assertNull(calibrator.getEstimatedMxy());
22115         assertNull(calibrator.getEstimatedMxz());
22116         assertNull(calibrator.getEstimatedMyx());
22117         assertNull(calibrator.getEstimatedMyz());
22118         assertNull(calibrator.getEstimatedMzx());
22119         assertNull(calibrator.getEstimatedMzy());
22120         assertNull(calibrator.getEstimatedCovariance());
22121         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22122     }
22123 
22124     @Test
22125     public void testConstructor184() throws WrongSizeException {
22126         final Matrix ba = generateBa();
22127         final double biasX = ba.getElementAtIndex(0);
22128         final double biasY = ba.getElementAtIndex(1);
22129         final double biasZ = ba.getElementAtIndex(2);
22130 
22131         final Matrix ma = generateMaCommonAxis();
22132         final double sx = ma.getElementAt(0, 0);
22133         final double sy = ma.getElementAt(1, 1);
22134         final double sz = ma.getElementAt(2, 2);
22135 
22136         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22137         final double latitude = Math.toRadians(
22138                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22139         final double longitude = Math.toRadians(
22140                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22141         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22142         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22143         final NEDVelocity nedVelocity = new NEDVelocity();
22144         final ECEFPosition ecefPosition = new ECEFPosition();
22145         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22146         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22147                 ecefPosition, ecefVelocity);
22148 
22149         final KnownPositionAccelerometerCalibrator calibrator =
22150                 new KnownPositionAccelerometerCalibrator(nedPosition,
22151                         true, biasX, biasY, biasZ, sx, sy, sz);
22152 
22153         // check default values
22154         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22155         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22156         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22157         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22158         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22159         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22160         final Acceleration bx2 = new Acceleration(0.0,
22161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22162         calibrator.getInitialBiasXAsAcceleration(bx2);
22163         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22164         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22165         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22166         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22167         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22168         final Acceleration by2 = new Acceleration(0.0,
22169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22170         calibrator.getInitialBiasYAsAcceleration(by2);
22171         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22172         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22173         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22174         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22175         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22176         final Acceleration bz2 = new Acceleration(0.0,
22177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22178         calibrator.getInitialBiasZAsAcceleration(bz2);
22179         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22180         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22181         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22182         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22183         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22184         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22185         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22186         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22187         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22188         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22189         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22190         final double[] bias1 = calibrator.getInitialBias();
22191         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22192         final double[] bias2 = new double[3];
22193         calibrator.getInitialBias(bias2);
22194         assertArrayEquals(bias1, bias2, 0.0);
22195         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22196         assertEquals(b1, ba);
22197         final Matrix b2 = new Matrix(3, 1);
22198         calibrator.getInitialBiasAsMatrix(b2);
22199         assertEquals(b1, b2);
22200         final Matrix ma1 = calibrator.getInitialMa();
22201         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22202         final Matrix ma2 = new Matrix(3, 3);
22203         calibrator.getInitialMa(ma2);
22204         assertEquals(ma1, ma2);
22205         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22206         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22207         final NEDPosition nedPosition1 = new NEDPosition();
22208         assertTrue(calibrator.getNedPosition(nedPosition1));
22209         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22210         assertNull(calibrator.getMeasurements());
22211         assertTrue(calibrator.isCommonAxisUsed());
22212         assertNull(calibrator.getListener());
22213         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22214         assertFalse(calibrator.isReady());
22215         assertFalse(calibrator.isRunning());
22216         assertNull(calibrator.getEstimatedBiases());
22217         assertFalse(calibrator.getEstimatedBiases(null));
22218         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22219         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22220         assertNull(calibrator.getEstimatedBiasFx());
22221         assertNull(calibrator.getEstimatedBiasFy());
22222         assertNull(calibrator.getEstimatedBiasFz());
22223         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22224         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22225         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22226         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22227         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22228         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22229         assertNull(calibrator.getEstimatedMa());
22230         assertNull(calibrator.getEstimatedSx());
22231         assertNull(calibrator.getEstimatedSy());
22232         assertNull(calibrator.getEstimatedSz());
22233         assertNull(calibrator.getEstimatedMxy());
22234         assertNull(calibrator.getEstimatedMxz());
22235         assertNull(calibrator.getEstimatedMyx());
22236         assertNull(calibrator.getEstimatedMyz());
22237         assertNull(calibrator.getEstimatedMzx());
22238         assertNull(calibrator.getEstimatedMzy());
22239         assertNull(calibrator.getEstimatedCovariance());
22240         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22241     }
22242 
22243     @Test
22244     public void testConstructor185() throws WrongSizeException {
22245         final Matrix ba = generateBa();
22246         final double biasX = ba.getElementAtIndex(0);
22247         final double biasY = ba.getElementAtIndex(1);
22248         final double biasZ = ba.getElementAtIndex(2);
22249 
22250         final Matrix ma = generateMaCommonAxis();
22251         final double sx = ma.getElementAt(0, 0);
22252         final double sy = ma.getElementAt(1, 1);
22253         final double sz = ma.getElementAt(2, 2);
22254 
22255         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22256         final double latitude = Math.toRadians(
22257                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22258         final double longitude = Math.toRadians(
22259                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22260         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22261         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22262         final NEDVelocity nedVelocity = new NEDVelocity();
22263         final ECEFPosition ecefPosition = new ECEFPosition();
22264         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22265         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22266                 ecefPosition, ecefVelocity);
22267 
22268         final KnownPositionAccelerometerCalibrator calibrator =
22269                 new KnownPositionAccelerometerCalibrator(nedPosition,
22270                         true, biasX, biasY, biasZ, sx, sy, sz,
22271                         this);
22272 
22273         // check default values
22274         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22275         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22276         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22277         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22278         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22279         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22280         final Acceleration bx2 = new Acceleration(0.0,
22281                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22282         calibrator.getInitialBiasXAsAcceleration(bx2);
22283         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22284         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22285         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22286         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22287         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22288         final Acceleration by2 = new Acceleration(0.0,
22289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22290         calibrator.getInitialBiasYAsAcceleration(by2);
22291         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22292         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22293         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22294         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22295         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22296         final Acceleration bz2 = new Acceleration(0.0,
22297                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22298         calibrator.getInitialBiasZAsAcceleration(bz2);
22299         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22300         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22301         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22302         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22303         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22304         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22305         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22306         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22307         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22308         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22309         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22310         final double[] bias1 = calibrator.getInitialBias();
22311         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22312         final double[] bias2 = new double[3];
22313         calibrator.getInitialBias(bias2);
22314         assertArrayEquals(bias1, bias2, 0.0);
22315         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22316         assertEquals(b1, ba);
22317         final Matrix b2 = new Matrix(3, 1);
22318         calibrator.getInitialBiasAsMatrix(b2);
22319         assertEquals(b1, b2);
22320         final Matrix ma1 = calibrator.getInitialMa();
22321         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22322         final Matrix ma2 = new Matrix(3, 3);
22323         calibrator.getInitialMa(ma2);
22324         assertEquals(ma1, ma2);
22325         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22326         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22327         final NEDPosition nedPosition1 = new NEDPosition();
22328         assertTrue(calibrator.getNedPosition(nedPosition1));
22329         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22330         assertNull(calibrator.getMeasurements());
22331         assertTrue(calibrator.isCommonAxisUsed());
22332         assertSame(calibrator.getListener(), this);
22333         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22334         assertFalse(calibrator.isReady());
22335         assertFalse(calibrator.isRunning());
22336         assertNull(calibrator.getEstimatedBiases());
22337         assertFalse(calibrator.getEstimatedBiases(null));
22338         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22339         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22340         assertNull(calibrator.getEstimatedBiasFx());
22341         assertNull(calibrator.getEstimatedBiasFy());
22342         assertNull(calibrator.getEstimatedBiasFz());
22343         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22344         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22345         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22346         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22347         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22348         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22349         assertNull(calibrator.getEstimatedMa());
22350         assertNull(calibrator.getEstimatedSx());
22351         assertNull(calibrator.getEstimatedSy());
22352         assertNull(calibrator.getEstimatedSz());
22353         assertNull(calibrator.getEstimatedMxy());
22354         assertNull(calibrator.getEstimatedMxz());
22355         assertNull(calibrator.getEstimatedMyx());
22356         assertNull(calibrator.getEstimatedMyz());
22357         assertNull(calibrator.getEstimatedMzx());
22358         assertNull(calibrator.getEstimatedMzy());
22359         assertNull(calibrator.getEstimatedCovariance());
22360         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22361     }
22362 
22363     @Test
22364     public void testConstructor186() throws WrongSizeException {
22365         final Collection<StandardDeviationBodyKinematics> measurements =
22366                 Collections.emptyList();
22367 
22368         final Matrix ba = generateBa();
22369         final double biasX = ba.getElementAtIndex(0);
22370         final double biasY = ba.getElementAtIndex(1);
22371         final double biasZ = ba.getElementAtIndex(2);
22372 
22373         final Matrix ma = generateMaCommonAxis();
22374         final double sx = ma.getElementAt(0, 0);
22375         final double sy = ma.getElementAt(1, 1);
22376         final double sz = ma.getElementAt(2, 2);
22377 
22378         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22379         final double latitude = Math.toRadians(
22380                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22381         final double longitude = Math.toRadians(
22382                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22383         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22384         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22385         final NEDVelocity nedVelocity = new NEDVelocity();
22386         final ECEFPosition ecefPosition = new ECEFPosition();
22387         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22388         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22389                 ecefPosition, ecefVelocity);
22390 
22391         final KnownPositionAccelerometerCalibrator calibrator =
22392                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22393                         true, biasX, biasY, biasZ, sx, sy, sz);
22394 
22395         // check default values
22396         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22397         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22398         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22399         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22400         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22401         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22402         final Acceleration bx2 = new Acceleration(0.0,
22403                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22404         calibrator.getInitialBiasXAsAcceleration(bx2);
22405         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22406         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22407         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22408         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22409         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22410         final Acceleration by2 = new Acceleration(0.0,
22411                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22412         calibrator.getInitialBiasYAsAcceleration(by2);
22413         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22414         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22415         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22416         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22417         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22418         final Acceleration bz2 = new Acceleration(0.0,
22419                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22420         calibrator.getInitialBiasZAsAcceleration(bz2);
22421         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22422         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22423         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22424         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22425         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22426         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22427         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22428         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22429         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22430         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22431         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22432         final double[] bias1 = calibrator.getInitialBias();
22433         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22434         final double[] bias2 = new double[3];
22435         calibrator.getInitialBias(bias2);
22436         assertArrayEquals(bias1, bias2, 0.0);
22437         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22438         assertEquals(b1, ba);
22439         final Matrix b2 = new Matrix(3, 1);
22440         calibrator.getInitialBiasAsMatrix(b2);
22441         assertEquals(b1, b2);
22442         final Matrix ma1 = calibrator.getInitialMa();
22443         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22444         final Matrix ma2 = new Matrix(3, 3);
22445         calibrator.getInitialMa(ma2);
22446         assertEquals(ma1, ma2);
22447         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22448         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22449         final NEDPosition nedPosition1 = new NEDPosition();
22450         assertTrue(calibrator.getNedPosition(nedPosition1));
22451         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22452         assertSame(calibrator.getMeasurements(), measurements);
22453         assertTrue(calibrator.isCommonAxisUsed());
22454         assertNull(calibrator.getListener());
22455         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22456         assertFalse(calibrator.isReady());
22457         assertFalse(calibrator.isRunning());
22458         assertNull(calibrator.getEstimatedBiases());
22459         assertFalse(calibrator.getEstimatedBiases(null));
22460         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22461         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22462         assertNull(calibrator.getEstimatedBiasFx());
22463         assertNull(calibrator.getEstimatedBiasFy());
22464         assertNull(calibrator.getEstimatedBiasFz());
22465         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22466         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22467         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22468         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22469         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22470         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22471         assertNull(calibrator.getEstimatedMa());
22472         assertNull(calibrator.getEstimatedSx());
22473         assertNull(calibrator.getEstimatedSy());
22474         assertNull(calibrator.getEstimatedSz());
22475         assertNull(calibrator.getEstimatedMxy());
22476         assertNull(calibrator.getEstimatedMxz());
22477         assertNull(calibrator.getEstimatedMyx());
22478         assertNull(calibrator.getEstimatedMyz());
22479         assertNull(calibrator.getEstimatedMzx());
22480         assertNull(calibrator.getEstimatedMzy());
22481         assertNull(calibrator.getEstimatedCovariance());
22482         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22483     }
22484 
22485     @Test
22486     public void testConstructor187() throws WrongSizeException {
22487         final Collection<StandardDeviationBodyKinematics> measurements =
22488                 Collections.emptyList();
22489 
22490         final Matrix ba = generateBa();
22491         final double biasX = ba.getElementAtIndex(0);
22492         final double biasY = ba.getElementAtIndex(1);
22493         final double biasZ = ba.getElementAtIndex(2);
22494 
22495         final Matrix ma = generateMaCommonAxis();
22496         final double sx = ma.getElementAt(0, 0);
22497         final double sy = ma.getElementAt(1, 1);
22498         final double sz = ma.getElementAt(2, 2);
22499 
22500         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22501         final double latitude = Math.toRadians(
22502                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22503         final double longitude = Math.toRadians(
22504                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22505         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22506         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22507         final NEDVelocity nedVelocity = new NEDVelocity();
22508         final ECEFPosition ecefPosition = new ECEFPosition();
22509         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22510         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22511                 ecefPosition, ecefVelocity);
22512 
22513         final KnownPositionAccelerometerCalibrator calibrator =
22514                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22515                         true, biasX, biasY, biasZ, sx, sy, sz,
22516                         this);
22517 
22518         // check default values
22519         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22520         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22521         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22522         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22523         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22524         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22525         final Acceleration bx2 = new Acceleration(0.0,
22526                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22527         calibrator.getInitialBiasXAsAcceleration(bx2);
22528         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22529         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22530         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22531         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22532         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22533         final Acceleration by2 = new Acceleration(0.0,
22534                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22535         calibrator.getInitialBiasYAsAcceleration(by2);
22536         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22537         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22538         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22539         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22540         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22541         final Acceleration bz2 = new Acceleration(0.0,
22542                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22543         calibrator.getInitialBiasZAsAcceleration(bz2);
22544         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22545         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22546         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22547         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22548         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22549         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22550         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22551         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22552         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22553         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22554         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22555         final double[] bias1 = calibrator.getInitialBias();
22556         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22557         final double[] bias2 = new double[3];
22558         calibrator.getInitialBias(bias2);
22559         assertArrayEquals(bias1, bias2, 0.0);
22560         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22561         assertEquals(b1, ba);
22562         final Matrix b2 = new Matrix(3, 1);
22563         calibrator.getInitialBiasAsMatrix(b2);
22564         assertEquals(b1, b2);
22565         final Matrix ma1 = calibrator.getInitialMa();
22566         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22567         final Matrix ma2 = new Matrix(3, 3);
22568         calibrator.getInitialMa(ma2);
22569         assertEquals(ma1, ma2);
22570         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22571         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22572         final NEDPosition nedPosition1 = new NEDPosition();
22573         assertTrue(calibrator.getNedPosition(nedPosition1));
22574         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22575         assertSame(calibrator.getMeasurements(), measurements);
22576         assertTrue(calibrator.isCommonAxisUsed());
22577         assertSame(calibrator.getListener(), this);
22578         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22579         assertFalse(calibrator.isReady());
22580         assertFalse(calibrator.isRunning());
22581         assertNull(calibrator.getEstimatedBiases());
22582         assertFalse(calibrator.getEstimatedBiases(null));
22583         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22584         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22585         assertNull(calibrator.getEstimatedBiasFx());
22586         assertNull(calibrator.getEstimatedBiasFy());
22587         assertNull(calibrator.getEstimatedBiasFz());
22588         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22589         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22590         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22591         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22592         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22593         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22594         assertNull(calibrator.getEstimatedMa());
22595         assertNull(calibrator.getEstimatedSx());
22596         assertNull(calibrator.getEstimatedSy());
22597         assertNull(calibrator.getEstimatedSz());
22598         assertNull(calibrator.getEstimatedMxy());
22599         assertNull(calibrator.getEstimatedMxz());
22600         assertNull(calibrator.getEstimatedMyx());
22601         assertNull(calibrator.getEstimatedMyz());
22602         assertNull(calibrator.getEstimatedMzx());
22603         assertNull(calibrator.getEstimatedMzy());
22604         assertNull(calibrator.getEstimatedCovariance());
22605         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22606     }
22607 
22608     @Test
22609     public void testConstructor188() throws WrongSizeException {
22610         final Matrix ba = generateBa();
22611         final double biasX = ba.getElementAtIndex(0);
22612         final double biasY = ba.getElementAtIndex(1);
22613         final double biasZ = ba.getElementAtIndex(2);
22614 
22615         final Acceleration bx = new Acceleration(biasX,
22616                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22617         final Acceleration by = new Acceleration(biasY,
22618                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22619         final Acceleration bz = new Acceleration(biasZ,
22620                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22621 
22622         final Matrix ma = generateMaCommonAxis();
22623         final double sx = ma.getElementAt(0, 0);
22624         final double sy = ma.getElementAt(1, 1);
22625         final double sz = ma.getElementAt(2, 2);
22626 
22627         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22628         final double latitude = Math.toRadians(
22629                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22630         final double longitude = Math.toRadians(
22631                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22632         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22633         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22634         final NEDVelocity nedVelocity = new NEDVelocity();
22635         final ECEFPosition ecefPosition = new ECEFPosition();
22636         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22637         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22638                 ecefPosition, ecefVelocity);
22639 
22640         final KnownPositionAccelerometerCalibrator calibrator =
22641                 new KnownPositionAccelerometerCalibrator(nedPosition,
22642                         bx, by, bz, sx, sy, sz);
22643 
22644         // check default values
22645         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22646         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22647         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22648         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22649         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22650         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22651         final Acceleration bx2 = new Acceleration(0.0,
22652                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22653         calibrator.getInitialBiasXAsAcceleration(bx2);
22654         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22655         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22656         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22657         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22658         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22659         final Acceleration by2 = new Acceleration(0.0,
22660                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22661         calibrator.getInitialBiasYAsAcceleration(by2);
22662         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22663         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22664         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22665         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22666         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22667         final Acceleration bz2 = new Acceleration(0.0,
22668                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22669         calibrator.getInitialBiasZAsAcceleration(bz2);
22670         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22671         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22672         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22673         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22674         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22675         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22676         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22677         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22678         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22679         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22680         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22681         final double[] bias1 = calibrator.getInitialBias();
22682         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22683         final double[] bias2 = new double[3];
22684         calibrator.getInitialBias(bias2);
22685         assertArrayEquals(bias1, bias2, 0.0);
22686         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22687         assertEquals(b1, ba);
22688         final Matrix b2 = new Matrix(3, 1);
22689         calibrator.getInitialBiasAsMatrix(b2);
22690         assertEquals(b1, b2);
22691         final Matrix ma1 = calibrator.getInitialMa();
22692         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22693         final Matrix ma2 = new Matrix(3, 3);
22694         calibrator.getInitialMa(ma2);
22695         assertEquals(ma1, ma2);
22696         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22697         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22698         final NEDPosition nedPosition1 = new NEDPosition();
22699         assertTrue(calibrator.getNedPosition(nedPosition1));
22700         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22701         assertNull(calibrator.getMeasurements());
22702         assertFalse(calibrator.isCommonAxisUsed());
22703         assertNull(calibrator.getListener());
22704         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22705         assertFalse(calibrator.isReady());
22706         assertFalse(calibrator.isRunning());
22707         assertNull(calibrator.getEstimatedBiases());
22708         assertFalse(calibrator.getEstimatedBiases(null));
22709         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22710         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22711         assertNull(calibrator.getEstimatedBiasFx());
22712         assertNull(calibrator.getEstimatedBiasFy());
22713         assertNull(calibrator.getEstimatedBiasFz());
22714         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22715         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22716         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22717         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22718         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22719         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22720         assertNull(calibrator.getEstimatedMa());
22721         assertNull(calibrator.getEstimatedSx());
22722         assertNull(calibrator.getEstimatedSy());
22723         assertNull(calibrator.getEstimatedSz());
22724         assertNull(calibrator.getEstimatedMxy());
22725         assertNull(calibrator.getEstimatedMxz());
22726         assertNull(calibrator.getEstimatedMyx());
22727         assertNull(calibrator.getEstimatedMyz());
22728         assertNull(calibrator.getEstimatedMzx());
22729         assertNull(calibrator.getEstimatedMzy());
22730         assertNull(calibrator.getEstimatedCovariance());
22731         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22732     }
22733 
22734     @Test
22735     public void testConstructor189() throws WrongSizeException {
22736         final Matrix ba = generateBa();
22737         final double biasX = ba.getElementAtIndex(0);
22738         final double biasY = ba.getElementAtIndex(1);
22739         final double biasZ = ba.getElementAtIndex(2);
22740 
22741         final Acceleration bx = new Acceleration(biasX,
22742                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743         final Acceleration by = new Acceleration(biasY,
22744                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22745         final Acceleration bz = new Acceleration(biasZ,
22746                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22747 
22748         final Matrix ma = generateMaCommonAxis();
22749         final double sx = ma.getElementAt(0, 0);
22750         final double sy = ma.getElementAt(1, 1);
22751         final double sz = ma.getElementAt(2, 2);
22752 
22753         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22754         final double latitude = Math.toRadians(
22755                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22756         final double longitude = Math.toRadians(
22757                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22758         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22759         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22760         final NEDVelocity nedVelocity = new NEDVelocity();
22761         final ECEFPosition ecefPosition = new ECEFPosition();
22762         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22763         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22764                 ecefPosition, ecefVelocity);
22765 
22766         final KnownPositionAccelerometerCalibrator calibrator =
22767                 new KnownPositionAccelerometerCalibrator(nedPosition,
22768                         bx, by, bz, sx, sy, sz, this);
22769 
22770         // check default values
22771         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22772         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22773         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22774         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22775         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22776         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22777         final Acceleration bx2 = new Acceleration(0.0,
22778                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22779         calibrator.getInitialBiasXAsAcceleration(bx2);
22780         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22781         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22783         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22784         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22785         final Acceleration by2 = new Acceleration(0.0,
22786                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22787         calibrator.getInitialBiasYAsAcceleration(by2);
22788         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22789         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22790         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22791         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22792         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22793         final Acceleration bz2 = new Acceleration(0.0,
22794                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22795         calibrator.getInitialBiasZAsAcceleration(bz2);
22796         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22797         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22798         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22799         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22800         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22801         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22802         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22803         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22804         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22805         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22806         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22807         final double[] bias1 = calibrator.getInitialBias();
22808         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22809         final double[] bias2 = new double[3];
22810         calibrator.getInitialBias(bias2);
22811         assertArrayEquals(bias1, bias2, 0.0);
22812         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22813         assertEquals(b1, ba);
22814         final Matrix b2 = new Matrix(3, 1);
22815         calibrator.getInitialBiasAsMatrix(b2);
22816         assertEquals(b1, b2);
22817         final Matrix ma1 = calibrator.getInitialMa();
22818         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22819         final Matrix ma2 = new Matrix(3, 3);
22820         calibrator.getInitialMa(ma2);
22821         assertEquals(ma1, ma2);
22822         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22823         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22824         final NEDPosition nedPosition1 = new NEDPosition();
22825         assertTrue(calibrator.getNedPosition(nedPosition1));
22826         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22827         assertNull(calibrator.getMeasurements());
22828         assertFalse(calibrator.isCommonAxisUsed());
22829         assertSame(calibrator.getListener(), this);
22830         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22831         assertFalse(calibrator.isReady());
22832         assertFalse(calibrator.isRunning());
22833         assertNull(calibrator.getEstimatedBiases());
22834         assertFalse(calibrator.getEstimatedBiases(null));
22835         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22836         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22837         assertNull(calibrator.getEstimatedBiasFx());
22838         assertNull(calibrator.getEstimatedBiasFy());
22839         assertNull(calibrator.getEstimatedBiasFz());
22840         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22841         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22842         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22843         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22844         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22845         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22846         assertNull(calibrator.getEstimatedMa());
22847         assertNull(calibrator.getEstimatedSx());
22848         assertNull(calibrator.getEstimatedSy());
22849         assertNull(calibrator.getEstimatedSz());
22850         assertNull(calibrator.getEstimatedMxy());
22851         assertNull(calibrator.getEstimatedMxz());
22852         assertNull(calibrator.getEstimatedMyx());
22853         assertNull(calibrator.getEstimatedMyz());
22854         assertNull(calibrator.getEstimatedMzx());
22855         assertNull(calibrator.getEstimatedMzy());
22856         assertNull(calibrator.getEstimatedCovariance());
22857         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22858     }
22859 
22860     @Test
22861     public void testConstructor190() throws WrongSizeException {
22862         final Collection<StandardDeviationBodyKinematics> measurements =
22863                 Collections.emptyList();
22864 
22865         final Matrix ba = generateBa();
22866         final double biasX = ba.getElementAtIndex(0);
22867         final double biasY = ba.getElementAtIndex(1);
22868         final double biasZ = ba.getElementAtIndex(2);
22869 
22870         final Acceleration bx = new Acceleration(biasX,
22871                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22872         final Acceleration by = new Acceleration(biasY,
22873                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22874         final Acceleration bz = new Acceleration(biasZ,
22875                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22876 
22877         final Matrix ma = generateMaCommonAxis();
22878         final double sx = ma.getElementAt(0, 0);
22879         final double sy = ma.getElementAt(1, 1);
22880         final double sz = ma.getElementAt(2, 2);
22881 
22882         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22883         final double latitude = Math.toRadians(
22884                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22885         final double longitude = Math.toRadians(
22886                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22887         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22888         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22889         final NEDVelocity nedVelocity = new NEDVelocity();
22890         final ECEFPosition ecefPosition = new ECEFPosition();
22891         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22892         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22893                 ecefPosition, ecefVelocity);
22894 
22895         final KnownPositionAccelerometerCalibrator calibrator =
22896                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22897                         bx, by, bz, sx, sy, sz);
22898 
22899         // check default values
22900         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22901         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22902         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22903         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22904         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22905         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22906         final Acceleration bx2 = new Acceleration(0.0,
22907                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22908         calibrator.getInitialBiasXAsAcceleration(bx2);
22909         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22910         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22911         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22912         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22913         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22914         final Acceleration by2 = new Acceleration(0.0,
22915                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22916         calibrator.getInitialBiasYAsAcceleration(by2);
22917         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22918         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22919         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22920         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22921         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22922         final Acceleration bz2 = new Acceleration(0.0,
22923                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22924         calibrator.getInitialBiasZAsAcceleration(bz2);
22925         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22926         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22927         assertEquals(calibrator.getInitialSx(), sx, 0.0);
22928         assertEquals(calibrator.getInitialSy(), sy, 0.0);
22929         assertEquals(calibrator.getInitialSz(), sz, 0.0);
22930         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22931         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22932         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22933         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22934         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22935         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22936         final double[] bias1 = calibrator.getInitialBias();
22937         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
22938         final double[] bias2 = new double[3];
22939         calibrator.getInitialBias(bias2);
22940         assertArrayEquals(bias1, bias2, 0.0);
22941         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22942         assertEquals(b1, ba);
22943         final Matrix b2 = new Matrix(3, 1);
22944         calibrator.getInitialBiasAsMatrix(b2);
22945         assertEquals(b1, b2);
22946         final Matrix ma1 = calibrator.getInitialMa();
22947         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
22948         final Matrix ma2 = new Matrix(3, 3);
22949         calibrator.getInitialMa(ma2);
22950         assertEquals(ma1, ma2);
22951         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22952         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22953         final NEDPosition nedPosition1 = new NEDPosition();
22954         assertTrue(calibrator.getNedPosition(nedPosition1));
22955         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22956         assertSame(calibrator.getMeasurements(), measurements);
22957         assertFalse(calibrator.isCommonAxisUsed());
22958         assertNull(calibrator.getListener());
22959         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22960         assertFalse(calibrator.isReady());
22961         assertFalse(calibrator.isRunning());
22962         assertNull(calibrator.getEstimatedBiases());
22963         assertFalse(calibrator.getEstimatedBiases(null));
22964         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22965         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22966         assertNull(calibrator.getEstimatedBiasFx());
22967         assertNull(calibrator.getEstimatedBiasFy());
22968         assertNull(calibrator.getEstimatedBiasFz());
22969         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22970         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22971         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22972         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22973         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22974         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22975         assertNull(calibrator.getEstimatedMa());
22976         assertNull(calibrator.getEstimatedSx());
22977         assertNull(calibrator.getEstimatedSy());
22978         assertNull(calibrator.getEstimatedSz());
22979         assertNull(calibrator.getEstimatedMxy());
22980         assertNull(calibrator.getEstimatedMxz());
22981         assertNull(calibrator.getEstimatedMyx());
22982         assertNull(calibrator.getEstimatedMyz());
22983         assertNull(calibrator.getEstimatedMzx());
22984         assertNull(calibrator.getEstimatedMzy());
22985         assertNull(calibrator.getEstimatedCovariance());
22986         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22987     }
22988 
22989     @Test
22990     public void testConstructor191() throws WrongSizeException {
22991         final Collection<StandardDeviationBodyKinematics> measurements =
22992                 Collections.emptyList();
22993 
22994         final Matrix ba = generateBa();
22995         final double biasX = ba.getElementAtIndex(0);
22996         final double biasY = ba.getElementAtIndex(1);
22997         final double biasZ = ba.getElementAtIndex(2);
22998 
22999         final Acceleration bx = new Acceleration(biasX,
23000                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23001         final Acceleration by = new Acceleration(biasY,
23002                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23003         final Acceleration bz = new Acceleration(biasZ,
23004                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23005 
23006         final Matrix ma = generateMaCommonAxis();
23007         final double sx = ma.getElementAt(0, 0);
23008         final double sy = ma.getElementAt(1, 1);
23009         final double sz = ma.getElementAt(2, 2);
23010 
23011         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23012         final double latitude = Math.toRadians(
23013                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23014         final double longitude = Math.toRadians(
23015                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23016         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23017         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23018         final NEDVelocity nedVelocity = new NEDVelocity();
23019         final ECEFPosition ecefPosition = new ECEFPosition();
23020         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23021         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23022                 ecefPosition, ecefVelocity);
23023 
23024         final KnownPositionAccelerometerCalibrator calibrator =
23025                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23026                         bx, by, bz, sx, sy, sz, this);
23027 
23028         // check default values
23029         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23030         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23031         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23032         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23033         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23034         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23035         final Acceleration bx2 = new Acceleration(0.0,
23036                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23037         calibrator.getInitialBiasXAsAcceleration(bx2);
23038         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23039         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23040         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23041         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23042         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23043         final Acceleration by2 = new Acceleration(0.0,
23044                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23045         calibrator.getInitialBiasYAsAcceleration(by2);
23046         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23047         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23048         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23049         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23050         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23051         final Acceleration bz2 = new Acceleration(0.0,
23052                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23053         calibrator.getInitialBiasZAsAcceleration(bz2);
23054         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23055         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23056         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23057         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23058         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23059         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23060         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23061         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23062         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23063         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23064         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23065         final double[] bias1 = calibrator.getInitialBias();
23066         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23067         final double[] bias2 = new double[3];
23068         calibrator.getInitialBias(bias2);
23069         assertArrayEquals(bias1, bias2, 0.0);
23070         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23071         assertEquals(b1, ba);
23072         final Matrix b2 = new Matrix(3, 1);
23073         calibrator.getInitialBiasAsMatrix(b2);
23074         assertEquals(b1, b2);
23075         final Matrix ma1 = calibrator.getInitialMa();
23076         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23077         final Matrix ma2 = new Matrix(3, 3);
23078         calibrator.getInitialMa(ma2);
23079         assertEquals(ma1, ma2);
23080         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23081         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23082         final NEDPosition nedPosition1 = new NEDPosition();
23083         assertTrue(calibrator.getNedPosition(nedPosition1));
23084         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23085         assertSame(calibrator.getMeasurements(), measurements);
23086         assertFalse(calibrator.isCommonAxisUsed());
23087         assertSame(calibrator.getListener(), this);
23088         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23089         assertFalse(calibrator.isReady());
23090         assertFalse(calibrator.isRunning());
23091         assertNull(calibrator.getEstimatedBiases());
23092         assertFalse(calibrator.getEstimatedBiases(null));
23093         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23094         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23095         assertNull(calibrator.getEstimatedBiasFx());
23096         assertNull(calibrator.getEstimatedBiasFy());
23097         assertNull(calibrator.getEstimatedBiasFz());
23098         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23099         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23100         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23101         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23102         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23103         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23104         assertNull(calibrator.getEstimatedMa());
23105         assertNull(calibrator.getEstimatedSx());
23106         assertNull(calibrator.getEstimatedSy());
23107         assertNull(calibrator.getEstimatedSz());
23108         assertNull(calibrator.getEstimatedMxy());
23109         assertNull(calibrator.getEstimatedMxz());
23110         assertNull(calibrator.getEstimatedMyx());
23111         assertNull(calibrator.getEstimatedMyz());
23112         assertNull(calibrator.getEstimatedMzx());
23113         assertNull(calibrator.getEstimatedMzy());
23114         assertNull(calibrator.getEstimatedCovariance());
23115         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23116     }
23117 
23118     @Test
23119     public void testConstructor192() throws WrongSizeException {
23120         final Matrix ba = generateBa();
23121         final double biasX = ba.getElementAtIndex(0);
23122         final double biasY = ba.getElementAtIndex(1);
23123         final double biasZ = ba.getElementAtIndex(2);
23124 
23125         final Acceleration bx = new Acceleration(biasX,
23126                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23127         final Acceleration by = new Acceleration(biasY,
23128                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23129         final Acceleration bz = new Acceleration(biasZ,
23130                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23131 
23132         final Matrix ma = generateMaCommonAxis();
23133         final double sx = ma.getElementAt(0, 0);
23134         final double sy = ma.getElementAt(1, 1);
23135         final double sz = ma.getElementAt(2, 2);
23136 
23137         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23138         final double latitude = Math.toRadians(
23139                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23140         final double longitude = Math.toRadians(
23141                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23142         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23143         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23144         final NEDVelocity nedVelocity = new NEDVelocity();
23145         final ECEFPosition ecefPosition = new ECEFPosition();
23146         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23147         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23148                 ecefPosition, ecefVelocity);
23149 
23150         final KnownPositionAccelerometerCalibrator calibrator =
23151                 new KnownPositionAccelerometerCalibrator(nedPosition,
23152                         true, bx, by, bz, sx, sy, sz);
23153 
23154         // check default values
23155         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23156         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23157         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23158         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23159         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23160         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23161         final Acceleration bx2 = new Acceleration(0.0,
23162                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23163         calibrator.getInitialBiasXAsAcceleration(bx2);
23164         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23165         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23166         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23167         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23168         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23169         final Acceleration by2 = new Acceleration(0.0,
23170                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23171         calibrator.getInitialBiasYAsAcceleration(by2);
23172         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23173         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23174         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23175         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23176         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23177         final Acceleration bz2 = new Acceleration(0.0,
23178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23179         calibrator.getInitialBiasZAsAcceleration(bz2);
23180         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23181         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23182         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23183         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23184         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23185         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23186         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23187         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23188         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23189         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23190         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23191         final double[] bias1 = calibrator.getInitialBias();
23192         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23193         final double[] bias2 = new double[3];
23194         calibrator.getInitialBias(bias2);
23195         assertArrayEquals(bias1, bias2, 0.0);
23196         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23197         assertEquals(b1, ba);
23198         final Matrix b2 = new Matrix(3, 1);
23199         calibrator.getInitialBiasAsMatrix(b2);
23200         assertEquals(b1, b2);
23201         final Matrix ma1 = calibrator.getInitialMa();
23202         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23203         final Matrix ma2 = new Matrix(3, 3);
23204         calibrator.getInitialMa(ma2);
23205         assertEquals(ma1, ma2);
23206         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23207         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23208         final NEDPosition nedPosition1 = new NEDPosition();
23209         assertTrue(calibrator.getNedPosition(nedPosition1));
23210         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23211         assertNull(calibrator.getMeasurements());
23212         assertTrue(calibrator.isCommonAxisUsed());
23213         assertNull(calibrator.getListener());
23214         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23215         assertFalse(calibrator.isReady());
23216         assertFalse(calibrator.isRunning());
23217         assertNull(calibrator.getEstimatedBiases());
23218         assertFalse(calibrator.getEstimatedBiases(null));
23219         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23220         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23221         assertNull(calibrator.getEstimatedBiasFx());
23222         assertNull(calibrator.getEstimatedBiasFy());
23223         assertNull(calibrator.getEstimatedBiasFz());
23224         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23225         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23226         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23227         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23228         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23229         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23230         assertNull(calibrator.getEstimatedMa());
23231         assertNull(calibrator.getEstimatedSx());
23232         assertNull(calibrator.getEstimatedSy());
23233         assertNull(calibrator.getEstimatedSz());
23234         assertNull(calibrator.getEstimatedMxy());
23235         assertNull(calibrator.getEstimatedMxz());
23236         assertNull(calibrator.getEstimatedMyx());
23237         assertNull(calibrator.getEstimatedMyz());
23238         assertNull(calibrator.getEstimatedMzx());
23239         assertNull(calibrator.getEstimatedMzy());
23240         assertNull(calibrator.getEstimatedCovariance());
23241         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23242     }
23243 
23244     @Test
23245     public void testConstructor193() throws WrongSizeException {
23246         final Matrix ba = generateBa();
23247         final double biasX = ba.getElementAtIndex(0);
23248         final double biasY = ba.getElementAtIndex(1);
23249         final double biasZ = ba.getElementAtIndex(2);
23250 
23251         final Acceleration bx = new Acceleration(biasX,
23252                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23253         final Acceleration by = new Acceleration(biasY,
23254                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23255         final Acceleration bz = new Acceleration(biasZ,
23256                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23257 
23258         final Matrix ma = generateMaCommonAxis();
23259         final double sx = ma.getElementAt(0, 0);
23260         final double sy = ma.getElementAt(1, 1);
23261         final double sz = ma.getElementAt(2, 2);
23262 
23263         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23264         final double latitude = Math.toRadians(
23265                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23266         final double longitude = Math.toRadians(
23267                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23268         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23269         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23270         final NEDVelocity nedVelocity = new NEDVelocity();
23271         final ECEFPosition ecefPosition = new ECEFPosition();
23272         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23273         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23274                 ecefPosition, ecefVelocity);
23275 
23276         final KnownPositionAccelerometerCalibrator calibrator =
23277                 new KnownPositionAccelerometerCalibrator(nedPosition,
23278                         true, bx, by, bz, sx, sy, sz, this);
23279 
23280         // check default values
23281         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23282         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23283         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23284         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23285         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23286         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23287         final Acceleration bx2 = new Acceleration(0.0,
23288                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23289         calibrator.getInitialBiasXAsAcceleration(bx2);
23290         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23291         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23292         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23293         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23294         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23295         final Acceleration by2 = new Acceleration(0.0,
23296                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23297         calibrator.getInitialBiasYAsAcceleration(by2);
23298         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23299         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23300         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23301         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23302         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23303         final Acceleration bz2 = new Acceleration(0.0,
23304                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23305         calibrator.getInitialBiasZAsAcceleration(bz2);
23306         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23307         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23308         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23309         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23310         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23311         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23312         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23313         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23314         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23315         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23316         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23317         final double[] bias1 = calibrator.getInitialBias();
23318         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23319         final double[] bias2 = new double[3];
23320         calibrator.getInitialBias(bias2);
23321         assertArrayEquals(bias1, bias2, 0.0);
23322         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23323         assertEquals(b1, ba);
23324         final Matrix b2 = new Matrix(3, 1);
23325         calibrator.getInitialBiasAsMatrix(b2);
23326         assertEquals(b1, b2);
23327         final Matrix ma1 = calibrator.getInitialMa();
23328         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23329         final Matrix ma2 = new Matrix(3, 3);
23330         calibrator.getInitialMa(ma2);
23331         assertEquals(ma1, ma2);
23332         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23333         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23334         final NEDPosition nedPosition1 = new NEDPosition();
23335         assertTrue(calibrator.getNedPosition(nedPosition1));
23336         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23337         assertNull(calibrator.getMeasurements());
23338         assertTrue(calibrator.isCommonAxisUsed());
23339         assertSame(calibrator.getListener(), this);
23340         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23341         assertFalse(calibrator.isReady());
23342         assertFalse(calibrator.isRunning());
23343         assertNull(calibrator.getEstimatedBiases());
23344         assertFalse(calibrator.getEstimatedBiases(null));
23345         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23346         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23347         assertNull(calibrator.getEstimatedBiasFx());
23348         assertNull(calibrator.getEstimatedBiasFy());
23349         assertNull(calibrator.getEstimatedBiasFz());
23350         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23351         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23352         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23353         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23354         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23355         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23356         assertNull(calibrator.getEstimatedMa());
23357         assertNull(calibrator.getEstimatedSx());
23358         assertNull(calibrator.getEstimatedSy());
23359         assertNull(calibrator.getEstimatedSz());
23360         assertNull(calibrator.getEstimatedMxy());
23361         assertNull(calibrator.getEstimatedMxz());
23362         assertNull(calibrator.getEstimatedMyx());
23363         assertNull(calibrator.getEstimatedMyz());
23364         assertNull(calibrator.getEstimatedMzx());
23365         assertNull(calibrator.getEstimatedMzy());
23366         assertNull(calibrator.getEstimatedCovariance());
23367         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23368     }
23369 
23370     @Test
23371     public void testConstructor194() throws WrongSizeException {
23372         final Collection<StandardDeviationBodyKinematics> measurements =
23373                 Collections.emptyList();
23374 
23375         final Matrix ba = generateBa();
23376         final double biasX = ba.getElementAtIndex(0);
23377         final double biasY = ba.getElementAtIndex(1);
23378         final double biasZ = ba.getElementAtIndex(2);
23379 
23380         final Acceleration bx = new Acceleration(biasX,
23381                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23382         final Acceleration by = new Acceleration(biasY,
23383                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23384         final Acceleration bz = new Acceleration(biasZ,
23385                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23386 
23387         final Matrix ma = generateMaCommonAxis();
23388         final double sx = ma.getElementAt(0, 0);
23389         final double sy = ma.getElementAt(1, 1);
23390         final double sz = ma.getElementAt(2, 2);
23391 
23392         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23393         final double latitude = Math.toRadians(
23394                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23395         final double longitude = Math.toRadians(
23396                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23397         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23398         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23399         final NEDVelocity nedVelocity = new NEDVelocity();
23400         final ECEFPosition ecefPosition = new ECEFPosition();
23401         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23402         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23403                 ecefPosition, ecefVelocity);
23404 
23405         final KnownPositionAccelerometerCalibrator calibrator =
23406                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23407                         true, bx, by, bz, sx, sy, sz);
23408 
23409         // check default values
23410         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23411         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23412         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23413         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23414         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23415         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23416         final Acceleration bx2 = new Acceleration(0.0,
23417                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23418         calibrator.getInitialBiasXAsAcceleration(bx2);
23419         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23420         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23421         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23422         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23423         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23424         final Acceleration by2 = new Acceleration(0.0,
23425                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23426         calibrator.getInitialBiasYAsAcceleration(by2);
23427         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23428         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23429         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23430         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23431         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23432         final Acceleration bz2 = new Acceleration(0.0,
23433                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23434         calibrator.getInitialBiasZAsAcceleration(bz2);
23435         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23436         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23437         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23438         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23439         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23440         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23441         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23442         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23443         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23444         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23445         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23446         final double[] bias1 = calibrator.getInitialBias();
23447         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23448         final double[] bias2 = new double[3];
23449         calibrator.getInitialBias(bias2);
23450         assertArrayEquals(bias1, bias2, 0.0);
23451         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23452         assertEquals(b1, ba);
23453         final Matrix b2 = new Matrix(3, 1);
23454         calibrator.getInitialBiasAsMatrix(b2);
23455         assertEquals(b1, b2);
23456         final Matrix ma1 = calibrator.getInitialMa();
23457         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23458         final Matrix ma2 = new Matrix(3, 3);
23459         calibrator.getInitialMa(ma2);
23460         assertEquals(ma1, ma2);
23461         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23462         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23463         final NEDPosition nedPosition1 = new NEDPosition();
23464         assertTrue(calibrator.getNedPosition(nedPosition1));
23465         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23466         assertSame(calibrator.getMeasurements(), measurements);
23467         assertTrue(calibrator.isCommonAxisUsed());
23468         assertNull(calibrator.getListener());
23469         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23470         assertFalse(calibrator.isReady());
23471         assertFalse(calibrator.isRunning());
23472         assertNull(calibrator.getEstimatedBiases());
23473         assertFalse(calibrator.getEstimatedBiases(null));
23474         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23475         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23476         assertNull(calibrator.getEstimatedBiasFx());
23477         assertNull(calibrator.getEstimatedBiasFy());
23478         assertNull(calibrator.getEstimatedBiasFz());
23479         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23480         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23481         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23482         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23483         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23484         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23485         assertNull(calibrator.getEstimatedMa());
23486         assertNull(calibrator.getEstimatedSx());
23487         assertNull(calibrator.getEstimatedSy());
23488         assertNull(calibrator.getEstimatedSz());
23489         assertNull(calibrator.getEstimatedMxy());
23490         assertNull(calibrator.getEstimatedMxz());
23491         assertNull(calibrator.getEstimatedMyx());
23492         assertNull(calibrator.getEstimatedMyz());
23493         assertNull(calibrator.getEstimatedMzx());
23494         assertNull(calibrator.getEstimatedMzy());
23495         assertNull(calibrator.getEstimatedCovariance());
23496         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23497     }
23498 
23499     @Test
23500     public void testConstructor195() throws WrongSizeException {
23501         final Collection<StandardDeviationBodyKinematics> measurements =
23502                 Collections.emptyList();
23503 
23504         final Matrix ba = generateBa();
23505         final double biasX = ba.getElementAtIndex(0);
23506         final double biasY = ba.getElementAtIndex(1);
23507         final double biasZ = ba.getElementAtIndex(2);
23508 
23509         final Acceleration bx = new Acceleration(biasX,
23510                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23511         final Acceleration by = new Acceleration(biasY,
23512                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23513         final Acceleration bz = new Acceleration(biasZ,
23514                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23515 
23516         final Matrix ma = generateMaCommonAxis();
23517         final double sx = ma.getElementAt(0, 0);
23518         final double sy = ma.getElementAt(1, 1);
23519         final double sz = ma.getElementAt(2, 2);
23520 
23521         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23522         final double latitude = Math.toRadians(
23523                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23524         final double longitude = Math.toRadians(
23525                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23526         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23527         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23528         final NEDVelocity nedVelocity = new NEDVelocity();
23529         final ECEFPosition ecefPosition = new ECEFPosition();
23530         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23531         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23532                 ecefPosition, ecefVelocity);
23533 
23534         final KnownPositionAccelerometerCalibrator calibrator =
23535                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23536                         true, bx, by, bz, sx, sy, sz, this);
23537 
23538         // check default values
23539         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23540         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23541         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23542         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23543         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23544         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23545         final Acceleration bx2 = new Acceleration(0.0,
23546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23547         calibrator.getInitialBiasXAsAcceleration(bx2);
23548         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23549         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23550         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23551         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23552         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23553         final Acceleration by2 = new Acceleration(0.0,
23554                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23555         calibrator.getInitialBiasYAsAcceleration(by2);
23556         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23557         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23558         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23559         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23560         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23561         final Acceleration bz2 = new Acceleration(0.0,
23562                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23563         calibrator.getInitialBiasZAsAcceleration(bz2);
23564         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23565         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23566         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23567         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23568         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23569         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23570         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23571         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23572         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23573         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23574         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23575         final double[] bias1 = calibrator.getInitialBias();
23576         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23577         final double[] bias2 = new double[3];
23578         calibrator.getInitialBias(bias2);
23579         assertArrayEquals(bias1, bias2, 0.0);
23580         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23581         assertEquals(b1, ba);
23582         final Matrix b2 = new Matrix(3, 1);
23583         calibrator.getInitialBiasAsMatrix(b2);
23584         assertEquals(b1, b2);
23585         final Matrix ma1 = calibrator.getInitialMa();
23586         assertEquals(ma1, Matrix.diagonal(new double[]{ sx, sy, sz }));
23587         final Matrix ma2 = new Matrix(3, 3);
23588         calibrator.getInitialMa(ma2);
23589         assertEquals(ma1, ma2);
23590         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23591         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23592         final NEDPosition nedPosition1 = new NEDPosition();
23593         assertTrue(calibrator.getNedPosition(nedPosition1));
23594         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23595         assertSame(calibrator.getMeasurements(), measurements);
23596         assertTrue(calibrator.isCommonAxisUsed());
23597         assertSame(calibrator.getListener(), this);
23598         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23599         assertFalse(calibrator.isReady());
23600         assertFalse(calibrator.isRunning());
23601         assertNull(calibrator.getEstimatedBiases());
23602         assertFalse(calibrator.getEstimatedBiases(null));
23603         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23604         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23605         assertNull(calibrator.getEstimatedBiasFx());
23606         assertNull(calibrator.getEstimatedBiasFy());
23607         assertNull(calibrator.getEstimatedBiasFz());
23608         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23609         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23610         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23611         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23612         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23613         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23614         assertNull(calibrator.getEstimatedMa());
23615         assertNull(calibrator.getEstimatedSx());
23616         assertNull(calibrator.getEstimatedSy());
23617         assertNull(calibrator.getEstimatedSz());
23618         assertNull(calibrator.getEstimatedMxy());
23619         assertNull(calibrator.getEstimatedMxz());
23620         assertNull(calibrator.getEstimatedMyx());
23621         assertNull(calibrator.getEstimatedMyz());
23622         assertNull(calibrator.getEstimatedMzx());
23623         assertNull(calibrator.getEstimatedMzy());
23624         assertNull(calibrator.getEstimatedCovariance());
23625         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23626     }
23627 
23628     @Test
23629     public void testConstructor196() throws WrongSizeException {
23630         final Matrix ba = generateBa();
23631         final double biasX = ba.getElementAtIndex(0);
23632         final double biasY = ba.getElementAtIndex(1);
23633         final double biasZ = ba.getElementAtIndex(2);
23634 
23635         final Matrix ma = generateMaCommonAxis();
23636         final double sx = ma.getElementAt(0, 0);
23637         final double sy = ma.getElementAt(1, 1);
23638         final double sz = ma.getElementAt(2, 2);
23639         final double mxy = ma.getElementAt(0, 1);
23640         final double mxz = ma.getElementAt(0, 2);
23641         final double myx = ma.getElementAt(1, 0);
23642         final double myz = ma.getElementAt(1, 2);
23643         final double mzx = ma.getElementAt(2, 0);
23644         final double mzy = ma.getElementAt(2, 1);
23645 
23646         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23647         final double latitude = Math.toRadians(
23648                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23649         final double longitude = Math.toRadians(
23650                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23651         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23652         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23653         final NEDVelocity nedVelocity = new NEDVelocity();
23654         final ECEFPosition ecefPosition = new ECEFPosition();
23655         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23656         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23657                 ecefPosition, ecefVelocity);
23658 
23659         final KnownPositionAccelerometerCalibrator calibrator =
23660                 new KnownPositionAccelerometerCalibrator(nedPosition,
23661                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23662                         myx, myz, mzx, mzy);
23663 
23664         // check default values
23665         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23666         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23667         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23668         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23669         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23670         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23671         final Acceleration bx2 = new Acceleration(0.0,
23672                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23673         calibrator.getInitialBiasXAsAcceleration(bx2);
23674         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23675         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23676         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23677         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23678         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23679         final Acceleration by2 = new Acceleration(0.0,
23680                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23681         calibrator.getInitialBiasYAsAcceleration(by2);
23682         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23683         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23684         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23685         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23686         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23687         final Acceleration bz2 = new Acceleration(0.0,
23688                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23689         calibrator.getInitialBiasZAsAcceleration(bz2);
23690         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23691         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23692         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23693         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23694         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23695         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23696         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23697         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23698         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23699         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23700         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23701         final double[] bias1 = calibrator.getInitialBias();
23702         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23703         final double[] bias2 = new double[3];
23704         calibrator.getInitialBias(bias2);
23705         assertArrayEquals(bias1, bias2, 0.0);
23706         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23707         assertEquals(b1, ba);
23708         final Matrix b2 = new Matrix(3, 1);
23709         calibrator.getInitialBiasAsMatrix(b2);
23710         assertEquals(b1, b2);
23711         final Matrix ma1 = new Matrix(3, 3);
23712         ma1.setSubmatrix(0, 0,
23713                 2, 2,
23714                 new double[]{ sx, myx, mzx,
23715                         mxy, sy, mzy,
23716                         mxz, myz, sz});
23717         assertEquals(calibrator.getInitialMa(), ma1);
23718         final Matrix ma2 = new Matrix(3, 3);
23719         calibrator.getInitialMa(ma2);
23720         assertEquals(ma1, ma2);
23721         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23722         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23723         final NEDPosition nedPosition1 = new NEDPosition();
23724         assertTrue(calibrator.getNedPosition(nedPosition1));
23725         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23726         assertNull(calibrator.getMeasurements());
23727         assertFalse(calibrator.isCommonAxisUsed());
23728         assertNull(calibrator.getListener());
23729         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23730         assertFalse(calibrator.isReady());
23731         assertFalse(calibrator.isRunning());
23732         assertNull(calibrator.getEstimatedBiases());
23733         assertFalse(calibrator.getEstimatedBiases(null));
23734         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23735         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23736         assertNull(calibrator.getEstimatedBiasFx());
23737         assertNull(calibrator.getEstimatedBiasFy());
23738         assertNull(calibrator.getEstimatedBiasFz());
23739         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23740         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23741         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23742         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23743         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23744         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23745         assertNull(calibrator.getEstimatedMa());
23746         assertNull(calibrator.getEstimatedSx());
23747         assertNull(calibrator.getEstimatedSy());
23748         assertNull(calibrator.getEstimatedSz());
23749         assertNull(calibrator.getEstimatedMxy());
23750         assertNull(calibrator.getEstimatedMxz());
23751         assertNull(calibrator.getEstimatedMyx());
23752         assertNull(calibrator.getEstimatedMyz());
23753         assertNull(calibrator.getEstimatedMzx());
23754         assertNull(calibrator.getEstimatedMzy());
23755         assertNull(calibrator.getEstimatedCovariance());
23756         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23757     }
23758 
23759     @Test
23760     public void testConstructor197() throws WrongSizeException {
23761         final Collection<StandardDeviationBodyKinematics> measurements =
23762                 Collections.emptyList();
23763 
23764         final Matrix ba = generateBa();
23765         final double biasX = ba.getElementAtIndex(0);
23766         final double biasY = ba.getElementAtIndex(1);
23767         final double biasZ = ba.getElementAtIndex(2);
23768 
23769         final Matrix ma = generateMaCommonAxis();
23770         final double sx = ma.getElementAt(0, 0);
23771         final double sy = ma.getElementAt(1, 1);
23772         final double sz = ma.getElementAt(2, 2);
23773         final double mxy = ma.getElementAt(0, 1);
23774         final double mxz = ma.getElementAt(0, 2);
23775         final double myx = ma.getElementAt(1, 0);
23776         final double myz = ma.getElementAt(1, 2);
23777         final double mzx = ma.getElementAt(2, 0);
23778         final double mzy = ma.getElementAt(2, 1);
23779 
23780         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23781         final double latitude = Math.toRadians(
23782                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23783         final double longitude = Math.toRadians(
23784                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23785         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23786         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23787         final NEDVelocity nedVelocity = new NEDVelocity();
23788         final ECEFPosition ecefPosition = new ECEFPosition();
23789         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23790         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23791                 ecefPosition, ecefVelocity);
23792 
23793         final KnownPositionAccelerometerCalibrator calibrator =
23794                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23795                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23796                         myx, myz, mzx, mzy);
23797 
23798         // check default values
23799         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23800         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23801         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23802         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23803         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23804         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23805         final Acceleration bx2 = new Acceleration(0.0,
23806                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23807         calibrator.getInitialBiasXAsAcceleration(bx2);
23808         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23809         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23810         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23811         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23812         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23813         final Acceleration by2 = new Acceleration(0.0,
23814                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23815         calibrator.getInitialBiasYAsAcceleration(by2);
23816         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23817         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23818         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23819         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23820         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23821         final Acceleration bz2 = new Acceleration(0.0,
23822                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23823         calibrator.getInitialBiasZAsAcceleration(bz2);
23824         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23825         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23826         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23827         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23828         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23829         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23830         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23831         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23832         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23833         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23834         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23835         final double[] bias1 = calibrator.getInitialBias();
23836         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23837         final double[] bias2 = new double[3];
23838         calibrator.getInitialBias(bias2);
23839         assertArrayEquals(bias1, bias2, 0.0);
23840         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23841         assertEquals(b1, ba);
23842         final Matrix b2 = new Matrix(3, 1);
23843         calibrator.getInitialBiasAsMatrix(b2);
23844         assertEquals(b1, b2);
23845         final Matrix ma1 = new Matrix(3, 3);
23846         ma1.setSubmatrix(0, 0,
23847                 2, 2,
23848                 new double[]{ sx, myx, mzx,
23849                         mxy, sy, mzy,
23850                         mxz, myz, sz});
23851         assertEquals(calibrator.getInitialMa(), ma1);
23852         final Matrix ma2 = new Matrix(3, 3);
23853         calibrator.getInitialMa(ma2);
23854         assertEquals(ma1, ma2);
23855         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23856         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23857         final NEDPosition nedPosition1 = new NEDPosition();
23858         assertTrue(calibrator.getNedPosition(nedPosition1));
23859         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23860         assertSame(calibrator.getMeasurements(), measurements);
23861         assertFalse(calibrator.isCommonAxisUsed());
23862         assertNull(calibrator.getListener());
23863         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23864         assertFalse(calibrator.isReady());
23865         assertFalse(calibrator.isRunning());
23866         assertNull(calibrator.getEstimatedBiases());
23867         assertFalse(calibrator.getEstimatedBiases(null));
23868         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23869         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23870         assertNull(calibrator.getEstimatedBiasFx());
23871         assertNull(calibrator.getEstimatedBiasFy());
23872         assertNull(calibrator.getEstimatedBiasFz());
23873         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23874         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23875         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23876         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23877         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23878         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23879         assertNull(calibrator.getEstimatedMa());
23880         assertNull(calibrator.getEstimatedSx());
23881         assertNull(calibrator.getEstimatedSy());
23882         assertNull(calibrator.getEstimatedSz());
23883         assertNull(calibrator.getEstimatedMxy());
23884         assertNull(calibrator.getEstimatedMxz());
23885         assertNull(calibrator.getEstimatedMyx());
23886         assertNull(calibrator.getEstimatedMyz());
23887         assertNull(calibrator.getEstimatedMzx());
23888         assertNull(calibrator.getEstimatedMzy());
23889         assertNull(calibrator.getEstimatedCovariance());
23890         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23891     }
23892 
23893     @Test
23894     public void testConstructor198() throws WrongSizeException {
23895         final Collection<StandardDeviationBodyKinematics> measurements =
23896                 Collections.emptyList();
23897 
23898         final Matrix ba = generateBa();
23899         final double biasX = ba.getElementAtIndex(0);
23900         final double biasY = ba.getElementAtIndex(1);
23901         final double biasZ = ba.getElementAtIndex(2);
23902 
23903         final Matrix ma = generateMaCommonAxis();
23904         final double sx = ma.getElementAt(0, 0);
23905         final double sy = ma.getElementAt(1, 1);
23906         final double sz = ma.getElementAt(2, 2);
23907         final double mxy = ma.getElementAt(0, 1);
23908         final double mxz = ma.getElementAt(0, 2);
23909         final double myx = ma.getElementAt(1, 0);
23910         final double myz = ma.getElementAt(1, 2);
23911         final double mzx = ma.getElementAt(2, 0);
23912         final double mzy = ma.getElementAt(2, 1);
23913 
23914         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23915         final double latitude = Math.toRadians(
23916                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23917         final double longitude = Math.toRadians(
23918                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23919         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23920         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23921         final NEDVelocity nedVelocity = new NEDVelocity();
23922         final ECEFPosition ecefPosition = new ECEFPosition();
23923         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23924         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23925                 ecefPosition, ecefVelocity);
23926 
23927         final KnownPositionAccelerometerCalibrator calibrator =
23928                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23929                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23930                         myx, myz, mzx, mzy, this);
23931 
23932         // check default values
23933         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23934         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23935         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23936         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23937         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23938         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23939         final Acceleration bx2 = new Acceleration(0.0,
23940                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23941         calibrator.getInitialBiasXAsAcceleration(bx2);
23942         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23943         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23944         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23945         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23946         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23947         final Acceleration by2 = new Acceleration(0.0,
23948                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23949         calibrator.getInitialBiasYAsAcceleration(by2);
23950         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23951         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23952         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23953         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23954         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23955         final Acceleration bz2 = new Acceleration(0.0,
23956                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23957         calibrator.getInitialBiasZAsAcceleration(bz2);
23958         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23959         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23961         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23962         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23963         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
23964         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
23965         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
23966         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
23967         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
23968         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
23969         final double[] bias1 = calibrator.getInitialBias();
23970         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
23971         final double[] bias2 = new double[3];
23972         calibrator.getInitialBias(bias2);
23973         assertArrayEquals(bias1, bias2, 0.0);
23974         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23975         assertEquals(b1, ba);
23976         final Matrix b2 = new Matrix(3, 1);
23977         calibrator.getInitialBiasAsMatrix(b2);
23978         assertEquals(b1, b2);
23979         final Matrix ma1 = new Matrix(3, 3);
23980         ma1.setSubmatrix(0, 0,
23981                 2, 2,
23982                 new double[]{ sx, myx, mzx,
23983                         mxy, sy, mzy,
23984                         mxz, myz, sz});
23985         assertEquals(calibrator.getInitialMa(), ma1);
23986         final Matrix ma2 = new Matrix(3, 3);
23987         calibrator.getInitialMa(ma2);
23988         assertEquals(ma1, ma2);
23989         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23990         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23991         final NEDPosition nedPosition1 = new NEDPosition();
23992         assertTrue(calibrator.getNedPosition(nedPosition1));
23993         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23994         assertSame(calibrator.getMeasurements(), measurements);
23995         assertFalse(calibrator.isCommonAxisUsed());
23996         assertSame(calibrator.getListener(), this);
23997         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23998         assertFalse(calibrator.isReady());
23999         assertFalse(calibrator.isRunning());
24000         assertNull(calibrator.getEstimatedBiases());
24001         assertFalse(calibrator.getEstimatedBiases(null));
24002         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24003         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24004         assertNull(calibrator.getEstimatedBiasFx());
24005         assertNull(calibrator.getEstimatedBiasFy());
24006         assertNull(calibrator.getEstimatedBiasFz());
24007         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24008         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24009         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24010         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24011         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24012         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24013         assertNull(calibrator.getEstimatedMa());
24014         assertNull(calibrator.getEstimatedSx());
24015         assertNull(calibrator.getEstimatedSy());
24016         assertNull(calibrator.getEstimatedSz());
24017         assertNull(calibrator.getEstimatedMxy());
24018         assertNull(calibrator.getEstimatedMxz());
24019         assertNull(calibrator.getEstimatedMyx());
24020         assertNull(calibrator.getEstimatedMyz());
24021         assertNull(calibrator.getEstimatedMzx());
24022         assertNull(calibrator.getEstimatedMzy());
24023         assertNull(calibrator.getEstimatedCovariance());
24024         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24025     }
24026 
24027     @Test
24028     public void testConstructor199() throws WrongSizeException {
24029         final Matrix ba = generateBa();
24030         final double biasX = ba.getElementAtIndex(0);
24031         final double biasY = ba.getElementAtIndex(1);
24032         final double biasZ = ba.getElementAtIndex(2);
24033 
24034         final Matrix ma = generateMaCommonAxis();
24035         final double sx = ma.getElementAt(0, 0);
24036         final double sy = ma.getElementAt(1, 1);
24037         final double sz = ma.getElementAt(2, 2);
24038         final double mxy = ma.getElementAt(0, 1);
24039         final double mxz = ma.getElementAt(0, 2);
24040         final double myx = ma.getElementAt(1, 0);
24041         final double myz = ma.getElementAt(1, 2);
24042         final double mzx = ma.getElementAt(2, 0);
24043         final double mzy = ma.getElementAt(2, 1);
24044 
24045         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24046         final double latitude = Math.toRadians(
24047                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24048         final double longitude = Math.toRadians(
24049                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24050         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24051         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24052         final NEDVelocity nedVelocity = new NEDVelocity();
24053         final ECEFPosition ecefPosition = new ECEFPosition();
24054         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24055         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24056                 ecefPosition, ecefVelocity);
24057 
24058         final KnownPositionAccelerometerCalibrator calibrator =
24059                 new KnownPositionAccelerometerCalibrator(nedPosition,
24060                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24061                         myx, myz, mzx, mzy);
24062 
24063         // check default values
24064         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24065         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24066         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24067         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24068         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24069         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24070         final Acceleration bx2 = new Acceleration(0.0,
24071                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24072         calibrator.getInitialBiasXAsAcceleration(bx2);
24073         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24074         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24075         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24076         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24077         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24078         final Acceleration by2 = new Acceleration(0.0,
24079                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24080         calibrator.getInitialBiasYAsAcceleration(by2);
24081         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24082         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24083         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24084         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24085         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24086         final Acceleration bz2 = new Acceleration(0.0,
24087                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24088         calibrator.getInitialBiasZAsAcceleration(bz2);
24089         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24090         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24091         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24092         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24093         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24094         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24095         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24096         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24097         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24098         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24099         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24100         final double[] bias1 = calibrator.getInitialBias();
24101         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24102         final double[] bias2 = new double[3];
24103         calibrator.getInitialBias(bias2);
24104         assertArrayEquals(bias1, bias2, 0.0);
24105         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24106         assertEquals(b1, ba);
24107         final Matrix b2 = new Matrix(3, 1);
24108         calibrator.getInitialBiasAsMatrix(b2);
24109         assertEquals(b1, b2);
24110         final Matrix ma1 = new Matrix(3, 3);
24111         ma1.setSubmatrix(0, 0,
24112                 2, 2,
24113                 new double[]{ sx, myx, mzx,
24114                         mxy, sy, mzy,
24115                         mxz, myz, sz});
24116         assertEquals(calibrator.getInitialMa(), ma1);
24117         final Matrix ma2 = new Matrix(3, 3);
24118         calibrator.getInitialMa(ma2);
24119         assertEquals(ma1, ma2);
24120         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24121         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24122         final NEDPosition nedPosition1 = new NEDPosition();
24123         assertTrue(calibrator.getNedPosition(nedPosition1));
24124         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24125         assertNull(calibrator.getMeasurements());
24126         assertTrue(calibrator.isCommonAxisUsed());
24127         assertNull(calibrator.getListener());
24128         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24129         assertFalse(calibrator.isReady());
24130         assertFalse(calibrator.isRunning());
24131         assertNull(calibrator.getEstimatedBiases());
24132         assertFalse(calibrator.getEstimatedBiases(null));
24133         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24134         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24135         assertNull(calibrator.getEstimatedBiasFx());
24136         assertNull(calibrator.getEstimatedBiasFy());
24137         assertNull(calibrator.getEstimatedBiasFz());
24138         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24139         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24140         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24141         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24142         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24143         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24144         assertNull(calibrator.getEstimatedMa());
24145         assertNull(calibrator.getEstimatedSx());
24146         assertNull(calibrator.getEstimatedSy());
24147         assertNull(calibrator.getEstimatedSz());
24148         assertNull(calibrator.getEstimatedMxy());
24149         assertNull(calibrator.getEstimatedMxz());
24150         assertNull(calibrator.getEstimatedMyx());
24151         assertNull(calibrator.getEstimatedMyz());
24152         assertNull(calibrator.getEstimatedMzx());
24153         assertNull(calibrator.getEstimatedMzy());
24154         assertNull(calibrator.getEstimatedCovariance());
24155         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24156     }
24157 
24158     @Test
24159     public void testConstructor200() throws WrongSizeException {
24160         final Matrix ba = generateBa();
24161         final double biasX = ba.getElementAtIndex(0);
24162         final double biasY = ba.getElementAtIndex(1);
24163         final double biasZ = ba.getElementAtIndex(2);
24164 
24165         final Matrix ma = generateMaCommonAxis();
24166         final double sx = ma.getElementAt(0, 0);
24167         final double sy = ma.getElementAt(1, 1);
24168         final double sz = ma.getElementAt(2, 2);
24169         final double mxy = ma.getElementAt(0, 1);
24170         final double mxz = ma.getElementAt(0, 2);
24171         final double myx = ma.getElementAt(1, 0);
24172         final double myz = ma.getElementAt(1, 2);
24173         final double mzx = ma.getElementAt(2, 0);
24174         final double mzy = ma.getElementAt(2, 1);
24175 
24176         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24177         final double latitude = Math.toRadians(
24178                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24179         final double longitude = Math.toRadians(
24180                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24181         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24182         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24183         final NEDVelocity nedVelocity = new NEDVelocity();
24184         final ECEFPosition ecefPosition = new ECEFPosition();
24185         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24186         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24187                 ecefPosition, ecefVelocity);
24188 
24189         final KnownPositionAccelerometerCalibrator calibrator =
24190                 new KnownPositionAccelerometerCalibrator(nedPosition,
24191                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24192                         myx, myz, mzx, mzy, this);
24193 
24194         // check default values
24195         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24196         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24197         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24198         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24199         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24200         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24201         final Acceleration bx2 = new Acceleration(0.0,
24202                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24203         calibrator.getInitialBiasXAsAcceleration(bx2);
24204         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24205         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24206         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24207         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24208         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24209         final Acceleration by2 = new Acceleration(0.0,
24210                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24211         calibrator.getInitialBiasYAsAcceleration(by2);
24212         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24213         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24214         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24215         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24216         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24217         final Acceleration bz2 = new Acceleration(0.0,
24218                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24219         calibrator.getInitialBiasZAsAcceleration(bz2);
24220         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24221         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24222         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24223         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24224         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24225         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24226         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24227         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24228         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24229         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24230         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24231         final double[] bias1 = calibrator.getInitialBias();
24232         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24233         final double[] bias2 = new double[3];
24234         calibrator.getInitialBias(bias2);
24235         assertArrayEquals(bias1, bias2, 0.0);
24236         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24237         assertEquals(b1, ba);
24238         final Matrix b2 = new Matrix(3, 1);
24239         calibrator.getInitialBiasAsMatrix(b2);
24240         assertEquals(b1, b2);
24241         final Matrix ma1 = new Matrix(3, 3);
24242         ma1.setSubmatrix(0, 0,
24243                 2, 2,
24244                 new double[]{ sx, myx, mzx,
24245                         mxy, sy, mzy,
24246                         mxz, myz, sz});
24247         assertEquals(calibrator.getInitialMa(), ma1);
24248         final Matrix ma2 = new Matrix(3, 3);
24249         calibrator.getInitialMa(ma2);
24250         assertEquals(ma1, ma2);
24251         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24252         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24253         final NEDPosition nedPosition1 = new NEDPosition();
24254         assertTrue(calibrator.getNedPosition(nedPosition1));
24255         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24256         assertNull(calibrator.getMeasurements());
24257         assertTrue(calibrator.isCommonAxisUsed());
24258         assertSame(calibrator.getListener(), this);
24259         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24260         assertFalse(calibrator.isReady());
24261         assertFalse(calibrator.isRunning());
24262         assertNull(calibrator.getEstimatedBiases());
24263         assertFalse(calibrator.getEstimatedBiases(null));
24264         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24265         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24266         assertNull(calibrator.getEstimatedBiasFx());
24267         assertNull(calibrator.getEstimatedBiasFy());
24268         assertNull(calibrator.getEstimatedBiasFz());
24269         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24270         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24271         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24272         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24273         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24274         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24275         assertNull(calibrator.getEstimatedMa());
24276         assertNull(calibrator.getEstimatedSx());
24277         assertNull(calibrator.getEstimatedSy());
24278         assertNull(calibrator.getEstimatedSz());
24279         assertNull(calibrator.getEstimatedMxy());
24280         assertNull(calibrator.getEstimatedMxz());
24281         assertNull(calibrator.getEstimatedMyx());
24282         assertNull(calibrator.getEstimatedMyz());
24283         assertNull(calibrator.getEstimatedMzx());
24284         assertNull(calibrator.getEstimatedMzy());
24285         assertNull(calibrator.getEstimatedCovariance());
24286         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24287     }
24288 
24289     @Test
24290     public void testConstructor201() throws WrongSizeException {
24291         final Collection<StandardDeviationBodyKinematics> measurements =
24292                 Collections.emptyList();
24293 
24294         final Matrix ba = generateBa();
24295         final double biasX = ba.getElementAtIndex(0);
24296         final double biasY = ba.getElementAtIndex(1);
24297         final double biasZ = ba.getElementAtIndex(2);
24298 
24299         final Matrix ma = generateMaCommonAxis();
24300         final double sx = ma.getElementAt(0, 0);
24301         final double sy = ma.getElementAt(1, 1);
24302         final double sz = ma.getElementAt(2, 2);
24303         final double mxy = ma.getElementAt(0, 1);
24304         final double mxz = ma.getElementAt(0, 2);
24305         final double myx = ma.getElementAt(1, 0);
24306         final double myz = ma.getElementAt(1, 2);
24307         final double mzx = ma.getElementAt(2, 0);
24308         final double mzy = ma.getElementAt(2, 1);
24309 
24310         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24311         final double latitude = Math.toRadians(
24312                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24313         final double longitude = Math.toRadians(
24314                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24315         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24316         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24317         final NEDVelocity nedVelocity = new NEDVelocity();
24318         final ECEFPosition ecefPosition = new ECEFPosition();
24319         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24320         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24321                 ecefPosition, ecefVelocity);
24322 
24323         final KnownPositionAccelerometerCalibrator calibrator =
24324                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24325                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24326                         myx, myz, mzx, mzy);
24327 
24328         // check default values
24329         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24330         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24331         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24332         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24333         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24334         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24335         final Acceleration bx2 = new Acceleration(0.0,
24336                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24337         calibrator.getInitialBiasXAsAcceleration(bx2);
24338         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24339         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24340         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24341         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24342         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24343         final Acceleration by2 = new Acceleration(0.0,
24344                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24345         calibrator.getInitialBiasYAsAcceleration(by2);
24346         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24347         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24348         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24349         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24350         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24351         final Acceleration bz2 = new Acceleration(0.0,
24352                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24353         calibrator.getInitialBiasZAsAcceleration(bz2);
24354         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24355         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24356         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24357         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24358         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24359         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24360         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24361         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24362         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24363         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24364         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24365         final double[] bias1 = calibrator.getInitialBias();
24366         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24367         final double[] bias2 = new double[3];
24368         calibrator.getInitialBias(bias2);
24369         assertArrayEquals(bias1, bias2, 0.0);
24370         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24371         assertEquals(b1, ba);
24372         final Matrix b2 = new Matrix(3, 1);
24373         calibrator.getInitialBiasAsMatrix(b2);
24374         assertEquals(b1, b2);
24375         final Matrix ma1 = new Matrix(3, 3);
24376         ma1.setSubmatrix(0, 0,
24377                 2, 2,
24378                 new double[]{ sx, myx, mzx,
24379                         mxy, sy, mzy,
24380                         mxz, myz, sz});
24381         assertEquals(calibrator.getInitialMa(), ma1);
24382         final Matrix ma2 = new Matrix(3, 3);
24383         calibrator.getInitialMa(ma2);
24384         assertEquals(ma1, ma2);
24385         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24386         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24387         final NEDPosition nedPosition1 = new NEDPosition();
24388         assertTrue(calibrator.getNedPosition(nedPosition1));
24389         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24390         assertSame(calibrator.getMeasurements(), measurements);
24391         assertTrue(calibrator.isCommonAxisUsed());
24392         assertNull(calibrator.getListener());
24393         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24394         assertFalse(calibrator.isReady());
24395         assertFalse(calibrator.isRunning());
24396         assertNull(calibrator.getEstimatedBiases());
24397         assertFalse(calibrator.getEstimatedBiases(null));
24398         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24399         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24400         assertNull(calibrator.getEstimatedBiasFx());
24401         assertNull(calibrator.getEstimatedBiasFy());
24402         assertNull(calibrator.getEstimatedBiasFz());
24403         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24404         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24405         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24406         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24407         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24408         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24409         assertNull(calibrator.getEstimatedMa());
24410         assertNull(calibrator.getEstimatedSx());
24411         assertNull(calibrator.getEstimatedSy());
24412         assertNull(calibrator.getEstimatedSz());
24413         assertNull(calibrator.getEstimatedMxy());
24414         assertNull(calibrator.getEstimatedMxz());
24415         assertNull(calibrator.getEstimatedMyx());
24416         assertNull(calibrator.getEstimatedMyz());
24417         assertNull(calibrator.getEstimatedMzx());
24418         assertNull(calibrator.getEstimatedMzy());
24419         assertNull(calibrator.getEstimatedCovariance());
24420         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24421     }
24422 
24423     @Test
24424     public void testConstructor202() throws WrongSizeException {
24425         final Collection<StandardDeviationBodyKinematics> measurements =
24426                 Collections.emptyList();
24427 
24428         final Matrix ba = generateBa();
24429         final double biasX = ba.getElementAtIndex(0);
24430         final double biasY = ba.getElementAtIndex(1);
24431         final double biasZ = ba.getElementAtIndex(2);
24432 
24433         final Matrix ma = generateMaCommonAxis();
24434         final double sx = ma.getElementAt(0, 0);
24435         final double sy = ma.getElementAt(1, 1);
24436         final double sz = ma.getElementAt(2, 2);
24437         final double mxy = ma.getElementAt(0, 1);
24438         final double mxz = ma.getElementAt(0, 2);
24439         final double myx = ma.getElementAt(1, 0);
24440         final double myz = ma.getElementAt(1, 2);
24441         final double mzx = ma.getElementAt(2, 0);
24442         final double mzy = ma.getElementAt(2, 1);
24443 
24444         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24445         final double latitude = Math.toRadians(
24446                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24447         final double longitude = Math.toRadians(
24448                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24449         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24450         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24451         final NEDVelocity nedVelocity = new NEDVelocity();
24452         final ECEFPosition ecefPosition = new ECEFPosition();
24453         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24454         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24455                 ecefPosition, ecefVelocity);
24456 
24457         final KnownPositionAccelerometerCalibrator calibrator =
24458                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24459                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
24460                         myx, myz, mzx, mzy, this);
24461 
24462         // check default values
24463         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24464         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24465         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24466         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24467         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24468         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24469         final Acceleration bx2 = new Acceleration(0.0,
24470                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24471         calibrator.getInitialBiasXAsAcceleration(bx2);
24472         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24473         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24474         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24475         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24476         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24477         final Acceleration by2 = new Acceleration(0.0,
24478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24479         calibrator.getInitialBiasYAsAcceleration(by2);
24480         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24481         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24482         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24483         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24484         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24485         final Acceleration bz2 = new Acceleration(0.0,
24486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24487         calibrator.getInitialBiasZAsAcceleration(bz2);
24488         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24489         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24490         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24491         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24492         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24493         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24494         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24495         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24496         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24497         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24498         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24499         final double[] bias1 = calibrator.getInitialBias();
24500         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24501         final double[] bias2 = new double[3];
24502         calibrator.getInitialBias(bias2);
24503         assertArrayEquals(bias1, bias2, 0.0);
24504         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24505         assertEquals(b1, ba);
24506         final Matrix b2 = new Matrix(3, 1);
24507         calibrator.getInitialBiasAsMatrix(b2);
24508         assertEquals(b1, b2);
24509         final Matrix ma1 = new Matrix(3, 3);
24510         ma1.setSubmatrix(0, 0,
24511                 2, 2,
24512                 new double[]{ sx, myx, mzx,
24513                         mxy, sy, mzy,
24514                         mxz, myz, sz});
24515         assertEquals(calibrator.getInitialMa(), ma1);
24516         final Matrix ma2 = new Matrix(3, 3);
24517         calibrator.getInitialMa(ma2);
24518         assertEquals(ma1, ma2);
24519         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24520         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24521         final NEDPosition nedPosition1 = new NEDPosition();
24522         assertTrue(calibrator.getNedPosition(nedPosition1));
24523         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24524         assertSame(calibrator.getMeasurements(), measurements);
24525         assertTrue(calibrator.isCommonAxisUsed());
24526         assertSame(calibrator.getListener(), this);
24527         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24528         assertFalse(calibrator.isReady());
24529         assertFalse(calibrator.isRunning());
24530         assertNull(calibrator.getEstimatedBiases());
24531         assertFalse(calibrator.getEstimatedBiases(null));
24532         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24533         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24534         assertNull(calibrator.getEstimatedBiasFx());
24535         assertNull(calibrator.getEstimatedBiasFy());
24536         assertNull(calibrator.getEstimatedBiasFz());
24537         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24538         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24539         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24540         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24541         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24542         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24543         assertNull(calibrator.getEstimatedMa());
24544         assertNull(calibrator.getEstimatedSx());
24545         assertNull(calibrator.getEstimatedSy());
24546         assertNull(calibrator.getEstimatedSz());
24547         assertNull(calibrator.getEstimatedMxy());
24548         assertNull(calibrator.getEstimatedMxz());
24549         assertNull(calibrator.getEstimatedMyx());
24550         assertNull(calibrator.getEstimatedMyz());
24551         assertNull(calibrator.getEstimatedMzx());
24552         assertNull(calibrator.getEstimatedMzy());
24553         assertNull(calibrator.getEstimatedCovariance());
24554         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24555     }
24556 
24557     @Test
24558     public void testConstructor203() throws WrongSizeException {
24559         final Matrix ba = generateBa();
24560         final double biasX = ba.getElementAtIndex(0);
24561         final double biasY = ba.getElementAtIndex(1);
24562         final double biasZ = ba.getElementAtIndex(2);
24563 
24564         final Acceleration bx = new Acceleration(biasX,
24565                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24566         final Acceleration by = new Acceleration(biasY,
24567                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24568         final Acceleration bz = new Acceleration(biasZ,
24569                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24570 
24571         final Matrix ma = generateMaCommonAxis();
24572         final double sx = ma.getElementAt(0, 0);
24573         final double sy = ma.getElementAt(1, 1);
24574         final double sz = ma.getElementAt(2, 2);
24575         final double mxy = ma.getElementAt(0, 1);
24576         final double mxz = ma.getElementAt(0, 2);
24577         final double myx = ma.getElementAt(1, 0);
24578         final double myz = ma.getElementAt(1, 2);
24579         final double mzx = ma.getElementAt(2, 0);
24580         final double mzy = ma.getElementAt(2, 1);
24581 
24582         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24583         final double latitude = Math.toRadians(
24584                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24585         final double longitude = Math.toRadians(
24586                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24587         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24588         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24589         final NEDVelocity nedVelocity = new NEDVelocity();
24590         final ECEFPosition ecefPosition = new ECEFPosition();
24591         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24592         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24593                 ecefPosition, ecefVelocity);
24594 
24595         final KnownPositionAccelerometerCalibrator calibrator =
24596                 new KnownPositionAccelerometerCalibrator(nedPosition,
24597                         bx, by, bz, sx, sy, sz, mxy, mxz,
24598                         myx, myz, mzx, mzy);
24599 
24600         // check default values
24601         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24602         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24603         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24604         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24605         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24606         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24607         final Acceleration bx2 = new Acceleration(0.0,
24608                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24609         calibrator.getInitialBiasXAsAcceleration(bx2);
24610         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24611         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24612         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24613         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24614         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24615         final Acceleration by2 = new Acceleration(0.0,
24616                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24617         calibrator.getInitialBiasYAsAcceleration(by2);
24618         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24619         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24620         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24621         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24622         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24623         final Acceleration bz2 = new Acceleration(0.0,
24624                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24625         calibrator.getInitialBiasZAsAcceleration(bz2);
24626         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24627         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24628         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24629         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24630         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24631         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24632         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24633         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24634         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24635         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24636         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24637         final double[] bias1 = calibrator.getInitialBias();
24638         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24639         final double[] bias2 = new double[3];
24640         calibrator.getInitialBias(bias2);
24641         assertArrayEquals(bias1, bias2, 0.0);
24642         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24643         assertEquals(b1, ba);
24644         final Matrix b2 = new Matrix(3, 1);
24645         calibrator.getInitialBiasAsMatrix(b2);
24646         assertEquals(b1, b2);
24647         final Matrix ma1 = new Matrix(3, 3);
24648         ma1.setSubmatrix(0, 0,
24649                 2, 2,
24650                 new double[]{ sx, myx, mzx,
24651                         mxy, sy, mzy,
24652                         mxz, myz, sz});
24653         assertEquals(calibrator.getInitialMa(), ma1);
24654         final Matrix ma2 = new Matrix(3, 3);
24655         calibrator.getInitialMa(ma2);
24656         assertEquals(ma1, ma2);
24657         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24658         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24659         final NEDPosition nedPosition1 = new NEDPosition();
24660         assertTrue(calibrator.getNedPosition(nedPosition1));
24661         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24662         assertNull(calibrator.getMeasurements());
24663         assertFalse(calibrator.isCommonAxisUsed());
24664         assertNull(calibrator.getListener());
24665         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24666         assertFalse(calibrator.isReady());
24667         assertFalse(calibrator.isRunning());
24668         assertNull(calibrator.getEstimatedBiases());
24669         assertFalse(calibrator.getEstimatedBiases(null));
24670         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24671         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24672         assertNull(calibrator.getEstimatedBiasFx());
24673         assertNull(calibrator.getEstimatedBiasFy());
24674         assertNull(calibrator.getEstimatedBiasFz());
24675         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24676         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24677         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24678         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24679         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24680         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24681         assertNull(calibrator.getEstimatedMa());
24682         assertNull(calibrator.getEstimatedSx());
24683         assertNull(calibrator.getEstimatedSy());
24684         assertNull(calibrator.getEstimatedSz());
24685         assertNull(calibrator.getEstimatedMxy());
24686         assertNull(calibrator.getEstimatedMxz());
24687         assertNull(calibrator.getEstimatedMyx());
24688         assertNull(calibrator.getEstimatedMyz());
24689         assertNull(calibrator.getEstimatedMzx());
24690         assertNull(calibrator.getEstimatedMzy());
24691         assertNull(calibrator.getEstimatedCovariance());
24692         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24693     }
24694 
24695     @Test
24696     public void testConstructor204() throws WrongSizeException {
24697         final Matrix ba = generateBa();
24698         final double biasX = ba.getElementAtIndex(0);
24699         final double biasY = ba.getElementAtIndex(1);
24700         final double biasZ = ba.getElementAtIndex(2);
24701 
24702         final Acceleration bx = new Acceleration(biasX,
24703                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24704         final Acceleration by = new Acceleration(biasY,
24705                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24706         final Acceleration bz = new Acceleration(biasZ,
24707                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24708 
24709         final Matrix ma = generateMaCommonAxis();
24710         final double sx = ma.getElementAt(0, 0);
24711         final double sy = ma.getElementAt(1, 1);
24712         final double sz = ma.getElementAt(2, 2);
24713         final double mxy = ma.getElementAt(0, 1);
24714         final double mxz = ma.getElementAt(0, 2);
24715         final double myx = ma.getElementAt(1, 0);
24716         final double myz = ma.getElementAt(1, 2);
24717         final double mzx = ma.getElementAt(2, 0);
24718         final double mzy = ma.getElementAt(2, 1);
24719 
24720         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24721         final double latitude = Math.toRadians(
24722                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24723         final double longitude = Math.toRadians(
24724                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24725         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24726         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24727         final NEDVelocity nedVelocity = new NEDVelocity();
24728         final ECEFPosition ecefPosition = new ECEFPosition();
24729         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24730         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24731                 ecefPosition, ecefVelocity);
24732 
24733         final KnownPositionAccelerometerCalibrator calibrator =
24734                 new KnownPositionAccelerometerCalibrator(nedPosition,
24735                         bx, by, bz, sx, sy, sz, mxy, mxz,
24736                         myx, myz, mzx, mzy, this);
24737 
24738         // check default values
24739         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24740         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24741         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24742         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24743         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24744         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24745         final Acceleration bx2 = new Acceleration(0.0,
24746                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24747         calibrator.getInitialBiasXAsAcceleration(bx2);
24748         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24749         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24750         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24751         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24752         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24753         final Acceleration by2 = new Acceleration(0.0,
24754                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24755         calibrator.getInitialBiasYAsAcceleration(by2);
24756         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24757         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24758         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24759         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24760         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24761         final Acceleration bz2 = new Acceleration(0.0,
24762                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24763         calibrator.getInitialBiasZAsAcceleration(bz2);
24764         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24765         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24766         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24767         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24768         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24769         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24770         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24771         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24772         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24773         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24774         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24775         final double[] bias1 = calibrator.getInitialBias();
24776         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24777         final double[] bias2 = new double[3];
24778         calibrator.getInitialBias(bias2);
24779         assertArrayEquals(bias1, bias2, 0.0);
24780         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24781         assertEquals(b1, ba);
24782         final Matrix b2 = new Matrix(3, 1);
24783         calibrator.getInitialBiasAsMatrix(b2);
24784         assertEquals(b1, b2);
24785         final Matrix ma1 = new Matrix(3, 3);
24786         ma1.setSubmatrix(0, 0,
24787                 2, 2,
24788                 new double[]{ sx, myx, mzx,
24789                         mxy, sy, mzy,
24790                         mxz, myz, sz});
24791         assertEquals(calibrator.getInitialMa(), ma1);
24792         final Matrix ma2 = new Matrix(3, 3);
24793         calibrator.getInitialMa(ma2);
24794         assertEquals(ma1, ma2);
24795         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24796         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24797         final NEDPosition nedPosition1 = new NEDPosition();
24798         assertTrue(calibrator.getNedPosition(nedPosition1));
24799         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24800         assertNull(calibrator.getMeasurements());
24801         assertFalse(calibrator.isCommonAxisUsed());
24802         assertSame(calibrator.getListener(), this);
24803         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24804         assertFalse(calibrator.isReady());
24805         assertFalse(calibrator.isRunning());
24806         assertNull(calibrator.getEstimatedBiases());
24807         assertFalse(calibrator.getEstimatedBiases(null));
24808         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24809         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24810         assertNull(calibrator.getEstimatedBiasFx());
24811         assertNull(calibrator.getEstimatedBiasFy());
24812         assertNull(calibrator.getEstimatedBiasFz());
24813         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24814         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24815         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24816         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24817         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24818         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24819         assertNull(calibrator.getEstimatedMa());
24820         assertNull(calibrator.getEstimatedSx());
24821         assertNull(calibrator.getEstimatedSy());
24822         assertNull(calibrator.getEstimatedSz());
24823         assertNull(calibrator.getEstimatedMxy());
24824         assertNull(calibrator.getEstimatedMxz());
24825         assertNull(calibrator.getEstimatedMyx());
24826         assertNull(calibrator.getEstimatedMyz());
24827         assertNull(calibrator.getEstimatedMzx());
24828         assertNull(calibrator.getEstimatedMzy());
24829         assertNull(calibrator.getEstimatedCovariance());
24830         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24831     }
24832 
24833     @Test
24834     public void testConstructor205() throws WrongSizeException {
24835         final Collection<StandardDeviationBodyKinematics> measurements =
24836                 Collections.emptyList();
24837 
24838         final Matrix ba = generateBa();
24839         final double biasX = ba.getElementAtIndex(0);
24840         final double biasY = ba.getElementAtIndex(1);
24841         final double biasZ = ba.getElementAtIndex(2);
24842 
24843         final Acceleration bx = new Acceleration(biasX,
24844                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24845         final Acceleration by = new Acceleration(biasY,
24846                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24847         final Acceleration bz = new Acceleration(biasZ,
24848                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24849 
24850         final Matrix ma = generateMaCommonAxis();
24851         final double sx = ma.getElementAt(0, 0);
24852         final double sy = ma.getElementAt(1, 1);
24853         final double sz = ma.getElementAt(2, 2);
24854         final double mxy = ma.getElementAt(0, 1);
24855         final double mxz = ma.getElementAt(0, 2);
24856         final double myx = ma.getElementAt(1, 0);
24857         final double myz = ma.getElementAt(1, 2);
24858         final double mzx = ma.getElementAt(2, 0);
24859         final double mzy = ma.getElementAt(2, 1);
24860 
24861         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24862         final double latitude = Math.toRadians(
24863                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24864         final double longitude = Math.toRadians(
24865                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24866         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24867         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24868         final NEDVelocity nedVelocity = new NEDVelocity();
24869         final ECEFPosition ecefPosition = new ECEFPosition();
24870         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24871         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24872                 ecefPosition, ecefVelocity);
24873 
24874         final KnownPositionAccelerometerCalibrator calibrator =
24875                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24876                         bx, by, bz, sx, sy, sz, mxy, mxz,
24877                         myx, myz, mzx, mzy);
24878 
24879         // check default values
24880         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24881         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24882         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24883         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24884         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24885         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24886         final Acceleration bx2 = new Acceleration(0.0,
24887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24888         calibrator.getInitialBiasXAsAcceleration(bx2);
24889         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24890         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24891         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24892         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24893         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24894         final Acceleration by2 = new Acceleration(0.0,
24895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24896         calibrator.getInitialBiasYAsAcceleration(by2);
24897         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24898         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24899         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24900         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24901         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24902         final Acceleration bz2 = new Acceleration(0.0,
24903                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24904         calibrator.getInitialBiasZAsAcceleration(bz2);
24905         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24906         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24907         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24908         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24909         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24910         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
24911         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
24912         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
24913         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
24914         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
24915         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
24916         final double[] bias1 = calibrator.getInitialBias();
24917         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
24918         final double[] bias2 = new double[3];
24919         calibrator.getInitialBias(bias2);
24920         assertArrayEquals(bias1, bias2, 0.0);
24921         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24922         assertEquals(b1, ba);
24923         final Matrix b2 = new Matrix(3, 1);
24924         calibrator.getInitialBiasAsMatrix(b2);
24925         assertEquals(b1, b2);
24926         final Matrix ma1 = new Matrix(3, 3);
24927         ma1.setSubmatrix(0, 0,
24928                 2, 2,
24929                 new double[]{ sx, myx, mzx,
24930                         mxy, sy, mzy,
24931                         mxz, myz, sz});
24932         assertEquals(calibrator.getInitialMa(), ma1);
24933         final Matrix ma2 = new Matrix(3, 3);
24934         calibrator.getInitialMa(ma2);
24935         assertEquals(ma1, ma2);
24936         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24937         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24938         final NEDPosition nedPosition1 = new NEDPosition();
24939         assertTrue(calibrator.getNedPosition(nedPosition1));
24940         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24941         assertSame(calibrator.getMeasurements(), measurements);
24942         assertFalse(calibrator.isCommonAxisUsed());
24943         assertNull(calibrator.getListener());
24944         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24945         assertFalse(calibrator.isReady());
24946         assertFalse(calibrator.isRunning());
24947         assertNull(calibrator.getEstimatedBiases());
24948         assertFalse(calibrator.getEstimatedBiases(null));
24949         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24950         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24951         assertNull(calibrator.getEstimatedBiasFx());
24952         assertNull(calibrator.getEstimatedBiasFy());
24953         assertNull(calibrator.getEstimatedBiasFz());
24954         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24955         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24956         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24957         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24958         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24959         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24960         assertNull(calibrator.getEstimatedMa());
24961         assertNull(calibrator.getEstimatedSx());
24962         assertNull(calibrator.getEstimatedSy());
24963         assertNull(calibrator.getEstimatedSz());
24964         assertNull(calibrator.getEstimatedMxy());
24965         assertNull(calibrator.getEstimatedMxz());
24966         assertNull(calibrator.getEstimatedMyx());
24967         assertNull(calibrator.getEstimatedMyz());
24968         assertNull(calibrator.getEstimatedMzx());
24969         assertNull(calibrator.getEstimatedMzy());
24970         assertNull(calibrator.getEstimatedCovariance());
24971         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24972     }
24973 
24974     @Test
24975     public void testConstructor206() throws WrongSizeException {
24976         final Collection<StandardDeviationBodyKinematics> measurements =
24977                 Collections.emptyList();
24978 
24979         final Matrix ba = generateBa();
24980         final double biasX = ba.getElementAtIndex(0);
24981         final double biasY = ba.getElementAtIndex(1);
24982         final double biasZ = ba.getElementAtIndex(2);
24983 
24984         final Acceleration bx = new Acceleration(biasX,
24985                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24986         final Acceleration by = new Acceleration(biasY,
24987                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24988         final Acceleration bz = new Acceleration(biasZ,
24989                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24990 
24991         final Matrix ma = generateMaCommonAxis();
24992         final double sx = ma.getElementAt(0, 0);
24993         final double sy = ma.getElementAt(1, 1);
24994         final double sz = ma.getElementAt(2, 2);
24995         final double mxy = ma.getElementAt(0, 1);
24996         final double mxz = ma.getElementAt(0, 2);
24997         final double myx = ma.getElementAt(1, 0);
24998         final double myz = ma.getElementAt(1, 2);
24999         final double mzx = ma.getElementAt(2, 0);
25000         final double mzy = ma.getElementAt(2, 1);
25001 
25002         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25003         final double latitude = Math.toRadians(
25004                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25005         final double longitude = Math.toRadians(
25006                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25007         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25008         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25009         final NEDVelocity nedVelocity = new NEDVelocity();
25010         final ECEFPosition ecefPosition = new ECEFPosition();
25011         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25012         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25013                 ecefPosition, ecefVelocity);
25014 
25015         final KnownPositionAccelerometerCalibrator calibrator =
25016                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25017                         bx, by, bz, sx, sy, sz, mxy, mxz,
25018                         myx, myz, mzx, mzy, this);
25019 
25020         // check default values
25021         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25022         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25023         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25024         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25025         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25026         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25027         final Acceleration bx2 = new Acceleration(0.0,
25028                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25029         calibrator.getInitialBiasXAsAcceleration(bx2);
25030         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25031         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25032         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25033         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25034         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25035         final Acceleration by2 = new Acceleration(0.0,
25036                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25037         calibrator.getInitialBiasYAsAcceleration(by2);
25038         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25039         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25040         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25041         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25042         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25043         final Acceleration bz2 = new Acceleration(0.0,
25044                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25045         calibrator.getInitialBiasZAsAcceleration(bz2);
25046         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25047         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25048         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25049         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25050         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25051         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25052         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25053         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25054         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25055         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25056         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25057         final double[] bias1 = calibrator.getInitialBias();
25058         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25059         final double[] bias2 = new double[3];
25060         calibrator.getInitialBias(bias2);
25061         assertArrayEquals(bias1, bias2, 0.0);
25062         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25063         assertEquals(b1, ba);
25064         final Matrix b2 = new Matrix(3, 1);
25065         calibrator.getInitialBiasAsMatrix(b2);
25066         assertEquals(b1, b2);
25067         final Matrix ma1 = new Matrix(3, 3);
25068         ma1.setSubmatrix(0, 0,
25069                 2, 2,
25070                 new double[]{ sx, myx, mzx,
25071                         mxy, sy, mzy,
25072                         mxz, myz, sz});
25073         assertEquals(calibrator.getInitialMa(), ma1);
25074         final Matrix ma2 = new Matrix(3, 3);
25075         calibrator.getInitialMa(ma2);
25076         assertEquals(ma1, ma2);
25077         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25078         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25079         final NEDPosition nedPosition1 = new NEDPosition();
25080         assertTrue(calibrator.getNedPosition(nedPosition1));
25081         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25082         assertSame(calibrator.getMeasurements(), measurements);
25083         assertFalse(calibrator.isCommonAxisUsed());
25084         assertSame(calibrator.getListener(), this);
25085         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25086         assertFalse(calibrator.isReady());
25087         assertFalse(calibrator.isRunning());
25088         assertNull(calibrator.getEstimatedBiases());
25089         assertFalse(calibrator.getEstimatedBiases(null));
25090         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25091         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25092         assertNull(calibrator.getEstimatedBiasFx());
25093         assertNull(calibrator.getEstimatedBiasFy());
25094         assertNull(calibrator.getEstimatedBiasFz());
25095         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25096         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25097         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25098         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25099         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25100         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25101         assertNull(calibrator.getEstimatedMa());
25102         assertNull(calibrator.getEstimatedSx());
25103         assertNull(calibrator.getEstimatedSy());
25104         assertNull(calibrator.getEstimatedSz());
25105         assertNull(calibrator.getEstimatedMxy());
25106         assertNull(calibrator.getEstimatedMxz());
25107         assertNull(calibrator.getEstimatedMyx());
25108         assertNull(calibrator.getEstimatedMyz());
25109         assertNull(calibrator.getEstimatedMzx());
25110         assertNull(calibrator.getEstimatedMzy());
25111         assertNull(calibrator.getEstimatedCovariance());
25112         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25113     }
25114 
25115     @Test
25116     public void testConstructor207() throws WrongSizeException {
25117         final Matrix ba = generateBa();
25118         final double biasX = ba.getElementAtIndex(0);
25119         final double biasY = ba.getElementAtIndex(1);
25120         final double biasZ = ba.getElementAtIndex(2);
25121 
25122         final Acceleration bx = new Acceleration(biasX,
25123                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25124         final Acceleration by = new Acceleration(biasY,
25125                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25126         final Acceleration bz = new Acceleration(biasZ,
25127                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25128 
25129         final Matrix ma = generateMaCommonAxis();
25130         final double sx = ma.getElementAt(0, 0);
25131         final double sy = ma.getElementAt(1, 1);
25132         final double sz = ma.getElementAt(2, 2);
25133         final double mxy = ma.getElementAt(0, 1);
25134         final double mxz = ma.getElementAt(0, 2);
25135         final double myx = ma.getElementAt(1, 0);
25136         final double myz = ma.getElementAt(1, 2);
25137         final double mzx = ma.getElementAt(2, 0);
25138         final double mzy = ma.getElementAt(2, 1);
25139 
25140         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25141         final double latitude = Math.toRadians(
25142                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25143         final double longitude = Math.toRadians(
25144                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25145         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25146         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25147         final NEDVelocity nedVelocity = new NEDVelocity();
25148         final ECEFPosition ecefPosition = new ECEFPosition();
25149         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25150         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25151                 ecefPosition, ecefVelocity);
25152 
25153         final KnownPositionAccelerometerCalibrator calibrator =
25154                 new KnownPositionAccelerometerCalibrator(nedPosition,
25155                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25156                         myx, myz, mzx, mzy);
25157 
25158         // check default values
25159         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25160         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25161         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25162         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25163         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25164         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25165         final Acceleration bx2 = new Acceleration(0.0,
25166                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25167         calibrator.getInitialBiasXAsAcceleration(bx2);
25168         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25169         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25170         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25171         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25172         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25173         final Acceleration by2 = new Acceleration(0.0,
25174                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25175         calibrator.getInitialBiasYAsAcceleration(by2);
25176         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25177         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25178         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25179         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25180         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25181         final Acceleration bz2 = new Acceleration(0.0,
25182                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25183         calibrator.getInitialBiasZAsAcceleration(bz2);
25184         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25185         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25186         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25187         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25188         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25189         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25190         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25191         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25192         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25193         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25194         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25195         final double[] bias1 = calibrator.getInitialBias();
25196         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25197         final double[] bias2 = new double[3];
25198         calibrator.getInitialBias(bias2);
25199         assertArrayEquals(bias1, bias2, 0.0);
25200         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25201         assertEquals(b1, ba);
25202         final Matrix b2 = new Matrix(3, 1);
25203         calibrator.getInitialBiasAsMatrix(b2);
25204         assertEquals(b1, b2);
25205         final Matrix ma1 = new Matrix(3, 3);
25206         ma1.setSubmatrix(0, 0,
25207                 2, 2,
25208                 new double[]{ sx, myx, mzx,
25209                         mxy, sy, mzy,
25210                         mxz, myz, sz});
25211         assertEquals(calibrator.getInitialMa(), ma1);
25212         final Matrix ma2 = new Matrix(3, 3);
25213         calibrator.getInitialMa(ma2);
25214         assertEquals(ma1, ma2);
25215         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25216         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25217         final NEDPosition nedPosition1 = new NEDPosition();
25218         assertTrue(calibrator.getNedPosition(nedPosition1));
25219         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25220         assertNull(calibrator.getMeasurements());
25221         assertTrue(calibrator.isCommonAxisUsed());
25222         assertNull(calibrator.getListener());
25223         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25224         assertFalse(calibrator.isReady());
25225         assertFalse(calibrator.isRunning());
25226         assertNull(calibrator.getEstimatedBiases());
25227         assertFalse(calibrator.getEstimatedBiases(null));
25228         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25229         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25230         assertNull(calibrator.getEstimatedBiasFx());
25231         assertNull(calibrator.getEstimatedBiasFy());
25232         assertNull(calibrator.getEstimatedBiasFz());
25233         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25234         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25235         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25236         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25237         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25238         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25239         assertNull(calibrator.getEstimatedMa());
25240         assertNull(calibrator.getEstimatedSx());
25241         assertNull(calibrator.getEstimatedSy());
25242         assertNull(calibrator.getEstimatedSz());
25243         assertNull(calibrator.getEstimatedMxy());
25244         assertNull(calibrator.getEstimatedMxz());
25245         assertNull(calibrator.getEstimatedMyx());
25246         assertNull(calibrator.getEstimatedMyz());
25247         assertNull(calibrator.getEstimatedMzx());
25248         assertNull(calibrator.getEstimatedMzy());
25249         assertNull(calibrator.getEstimatedCovariance());
25250         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25251     }
25252 
25253     @Test
25254     public void testConstructor208() throws WrongSizeException {
25255         final Matrix ba = generateBa();
25256         final double biasX = ba.getElementAtIndex(0);
25257         final double biasY = ba.getElementAtIndex(1);
25258         final double biasZ = ba.getElementAtIndex(2);
25259 
25260         final Acceleration bx = new Acceleration(biasX,
25261                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25262         final Acceleration by = new Acceleration(biasY,
25263                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25264         final Acceleration bz = new Acceleration(biasZ,
25265                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25266 
25267         final Matrix ma = generateMaCommonAxis();
25268         final double sx = ma.getElementAt(0, 0);
25269         final double sy = ma.getElementAt(1, 1);
25270         final double sz = ma.getElementAt(2, 2);
25271         final double mxy = ma.getElementAt(0, 1);
25272         final double mxz = ma.getElementAt(0, 2);
25273         final double myx = ma.getElementAt(1, 0);
25274         final double myz = ma.getElementAt(1, 2);
25275         final double mzx = ma.getElementAt(2, 0);
25276         final double mzy = ma.getElementAt(2, 1);
25277 
25278         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25279         final double latitude = Math.toRadians(
25280                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25281         final double longitude = Math.toRadians(
25282                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25283         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25284         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25285         final NEDVelocity nedVelocity = new NEDVelocity();
25286         final ECEFPosition ecefPosition = new ECEFPosition();
25287         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25288         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25289                 ecefPosition, ecefVelocity);
25290 
25291         final KnownPositionAccelerometerCalibrator calibrator =
25292                 new KnownPositionAccelerometerCalibrator(nedPosition,
25293                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25294                         myx, myz, mzx, mzy, this);
25295 
25296         // check default values
25297         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25298         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25299         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25300         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25301         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25302         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25303         final Acceleration bx2 = new Acceleration(0.0,
25304                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25305         calibrator.getInitialBiasXAsAcceleration(bx2);
25306         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25307         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25308         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25309         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25310         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25311         final Acceleration by2 = new Acceleration(0.0,
25312                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25313         calibrator.getInitialBiasYAsAcceleration(by2);
25314         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25315         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25316         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25317         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25318         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25319         final Acceleration bz2 = new Acceleration(0.0,
25320                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25321         calibrator.getInitialBiasZAsAcceleration(bz2);
25322         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25323         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25324         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25325         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25326         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25327         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25328         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25329         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25330         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25331         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25332         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25333         final double[] bias1 = calibrator.getInitialBias();
25334         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25335         final double[] bias2 = new double[3];
25336         calibrator.getInitialBias(bias2);
25337         assertArrayEquals(bias1, bias2, 0.0);
25338         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25339         assertEquals(b1, ba);
25340         final Matrix b2 = new Matrix(3, 1);
25341         calibrator.getInitialBiasAsMatrix(b2);
25342         assertEquals(b1, b2);
25343         final Matrix ma1 = new Matrix(3, 3);
25344         ma1.setSubmatrix(0, 0,
25345                 2, 2,
25346                 new double[]{ sx, myx, mzx,
25347                         mxy, sy, mzy,
25348                         mxz, myz, sz});
25349         assertEquals(calibrator.getInitialMa(), ma1);
25350         final Matrix ma2 = new Matrix(3, 3);
25351         calibrator.getInitialMa(ma2);
25352         assertEquals(ma1, ma2);
25353         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25354         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25355         final NEDPosition nedPosition1 = new NEDPosition();
25356         assertTrue(calibrator.getNedPosition(nedPosition1));
25357         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25358         assertNull(calibrator.getMeasurements());
25359         assertTrue(calibrator.isCommonAxisUsed());
25360         assertSame(calibrator.getListener(), this);
25361         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25362         assertFalse(calibrator.isReady());
25363         assertFalse(calibrator.isRunning());
25364         assertNull(calibrator.getEstimatedBiases());
25365         assertFalse(calibrator.getEstimatedBiases(null));
25366         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25367         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25368         assertNull(calibrator.getEstimatedBiasFx());
25369         assertNull(calibrator.getEstimatedBiasFy());
25370         assertNull(calibrator.getEstimatedBiasFz());
25371         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25372         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25373         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25374         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25375         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25376         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25377         assertNull(calibrator.getEstimatedMa());
25378         assertNull(calibrator.getEstimatedSx());
25379         assertNull(calibrator.getEstimatedSy());
25380         assertNull(calibrator.getEstimatedSz());
25381         assertNull(calibrator.getEstimatedMxy());
25382         assertNull(calibrator.getEstimatedMxz());
25383         assertNull(calibrator.getEstimatedMyx());
25384         assertNull(calibrator.getEstimatedMyz());
25385         assertNull(calibrator.getEstimatedMzx());
25386         assertNull(calibrator.getEstimatedMzy());
25387         assertNull(calibrator.getEstimatedCovariance());
25388         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25389     }
25390 
25391     @Test
25392     public void testConstructor209() throws WrongSizeException {
25393         final Collection<StandardDeviationBodyKinematics> measurements =
25394                 Collections.emptyList();
25395 
25396         final Matrix ba = generateBa();
25397         final double biasX = ba.getElementAtIndex(0);
25398         final double biasY = ba.getElementAtIndex(1);
25399         final double biasZ = ba.getElementAtIndex(2);
25400 
25401         final Acceleration bx = new Acceleration(biasX,
25402                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25403         final Acceleration by = new Acceleration(biasY,
25404                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25405         final Acceleration bz = new Acceleration(biasZ,
25406                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25407 
25408         final Matrix ma = generateMaCommonAxis();
25409         final double sx = ma.getElementAt(0, 0);
25410         final double sy = ma.getElementAt(1, 1);
25411         final double sz = ma.getElementAt(2, 2);
25412         final double mxy = ma.getElementAt(0, 1);
25413         final double mxz = ma.getElementAt(0, 2);
25414         final double myx = ma.getElementAt(1, 0);
25415         final double myz = ma.getElementAt(1, 2);
25416         final double mzx = ma.getElementAt(2, 0);
25417         final double mzy = ma.getElementAt(2, 1);
25418 
25419         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25420         final double latitude = Math.toRadians(
25421                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25422         final double longitude = Math.toRadians(
25423                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25424         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25425         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25426         final NEDVelocity nedVelocity = new NEDVelocity();
25427         final ECEFPosition ecefPosition = new ECEFPosition();
25428         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25429         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25430                 ecefPosition, ecefVelocity);
25431 
25432         final KnownPositionAccelerometerCalibrator calibrator =
25433                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25434                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25435                         myx, myz, mzx, mzy);
25436 
25437         // check default values
25438         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25439         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25440         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25441         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25442         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25443         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25444         final Acceleration bx2 = new Acceleration(0.0,
25445                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25446         calibrator.getInitialBiasXAsAcceleration(bx2);
25447         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25448         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25449         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25450         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25451         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25452         final Acceleration by2 = new Acceleration(0.0,
25453                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25454         calibrator.getInitialBiasYAsAcceleration(by2);
25455         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25456         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25457         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25458         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25459         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25460         final Acceleration bz2 = new Acceleration(0.0,
25461                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25462         calibrator.getInitialBiasZAsAcceleration(bz2);
25463         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25464         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25465         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25466         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25467         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25468         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25469         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25470         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25471         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25472         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25473         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25474         final double[] bias1 = calibrator.getInitialBias();
25475         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25476         final double[] bias2 = new double[3];
25477         calibrator.getInitialBias(bias2);
25478         assertArrayEquals(bias1, bias2, 0.0);
25479         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25480         assertEquals(b1, ba);
25481         final Matrix b2 = new Matrix(3, 1);
25482         calibrator.getInitialBiasAsMatrix(b2);
25483         assertEquals(b1, b2);
25484         final Matrix ma1 = new Matrix(3, 3);
25485         ma1.setSubmatrix(0, 0,
25486                 2, 2,
25487                 new double[]{ sx, myx, mzx,
25488                         mxy, sy, mzy,
25489                         mxz, myz, sz});
25490         assertEquals(calibrator.getInitialMa(), ma1);
25491         final Matrix ma2 = new Matrix(3, 3);
25492         calibrator.getInitialMa(ma2);
25493         assertEquals(ma1, ma2);
25494         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25495         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25496         final NEDPosition nedPosition1 = new NEDPosition();
25497         assertTrue(calibrator.getNedPosition(nedPosition1));
25498         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25499         assertSame(calibrator.getMeasurements(), measurements);
25500         assertTrue(calibrator.isCommonAxisUsed());
25501         assertNull(calibrator.getListener());
25502         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25503         assertFalse(calibrator.isReady());
25504         assertFalse(calibrator.isRunning());
25505         assertNull(calibrator.getEstimatedBiases());
25506         assertFalse(calibrator.getEstimatedBiases(null));
25507         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25508         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25509         assertNull(calibrator.getEstimatedBiasFx());
25510         assertNull(calibrator.getEstimatedBiasFy());
25511         assertNull(calibrator.getEstimatedBiasFz());
25512         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25513         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25514         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25515         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25516         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25517         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25518         assertNull(calibrator.getEstimatedMa());
25519         assertNull(calibrator.getEstimatedSx());
25520         assertNull(calibrator.getEstimatedSy());
25521         assertNull(calibrator.getEstimatedSz());
25522         assertNull(calibrator.getEstimatedMxy());
25523         assertNull(calibrator.getEstimatedMxz());
25524         assertNull(calibrator.getEstimatedMyx());
25525         assertNull(calibrator.getEstimatedMyz());
25526         assertNull(calibrator.getEstimatedMzx());
25527         assertNull(calibrator.getEstimatedMzy());
25528         assertNull(calibrator.getEstimatedCovariance());
25529         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25530     }
25531 
25532     @Test
25533     public void testConstructor210() throws WrongSizeException {
25534         final Collection<StandardDeviationBodyKinematics> measurements =
25535                 Collections.emptyList();
25536 
25537         final Matrix ba = generateBa();
25538         final double biasX = ba.getElementAtIndex(0);
25539         final double biasY = ba.getElementAtIndex(1);
25540         final double biasZ = ba.getElementAtIndex(2);
25541 
25542         final Acceleration bx = new Acceleration(biasX,
25543                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25544         final Acceleration by = new Acceleration(biasY,
25545                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25546         final Acceleration bz = new Acceleration(biasZ,
25547                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25548 
25549         final Matrix ma = generateMaCommonAxis();
25550         final double sx = ma.getElementAt(0, 0);
25551         final double sy = ma.getElementAt(1, 1);
25552         final double sz = ma.getElementAt(2, 2);
25553         final double mxy = ma.getElementAt(0, 1);
25554         final double mxz = ma.getElementAt(0, 2);
25555         final double myx = ma.getElementAt(1, 0);
25556         final double myz = ma.getElementAt(1, 2);
25557         final double mzx = ma.getElementAt(2, 0);
25558         final double mzy = ma.getElementAt(2, 1);
25559 
25560         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25561         final double latitude = Math.toRadians(
25562                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25563         final double longitude = Math.toRadians(
25564                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25565         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25566         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25567         final NEDVelocity nedVelocity = new NEDVelocity();
25568         final ECEFPosition ecefPosition = new ECEFPosition();
25569         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25570         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25571                 ecefPosition, ecefVelocity);
25572 
25573         final KnownPositionAccelerometerCalibrator calibrator =
25574                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25575                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
25576                         myx, myz, mzx, mzy, this);
25577 
25578         // check default values
25579         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25580         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25581         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25582         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25583         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25584         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25585         final Acceleration bx2 = new Acceleration(0.0,
25586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25587         calibrator.getInitialBiasXAsAcceleration(bx2);
25588         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25589         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25590         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25591         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25592         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25593         final Acceleration by2 = new Acceleration(0.0,
25594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25595         calibrator.getInitialBiasYAsAcceleration(by2);
25596         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25597         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25598         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25599         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25600         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25601         final Acceleration bz2 = new Acceleration(0.0,
25602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25603         calibrator.getInitialBiasZAsAcceleration(bz2);
25604         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25605         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25606         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25607         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25608         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25609         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25610         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25611         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25612         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25613         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25614         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25615         final double[] bias1 = calibrator.getInitialBias();
25616         assertArrayEquals(bias1, new double[] { biasX, biasY, biasZ }, 0.0);
25617         final double[] bias2 = new double[3];
25618         calibrator.getInitialBias(bias2);
25619         assertArrayEquals(bias1, bias2, 0.0);
25620         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25621         assertEquals(b1, ba);
25622         final Matrix b2 = new Matrix(3, 1);
25623         calibrator.getInitialBiasAsMatrix(b2);
25624         assertEquals(b1, b2);
25625         final Matrix ma1 = new Matrix(3, 3);
25626         ma1.setSubmatrix(0, 0,
25627                 2, 2,
25628                 new double[]{ sx, myx, mzx,
25629                         mxy, sy, mzy,
25630                         mxz, myz, sz});
25631         assertEquals(calibrator.getInitialMa(), ma1);
25632         final Matrix ma2 = new Matrix(3, 3);
25633         calibrator.getInitialMa(ma2);
25634         assertEquals(ma1, ma2);
25635         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25636         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25637         final NEDPosition nedPosition1 = new NEDPosition();
25638         assertTrue(calibrator.getNedPosition(nedPosition1));
25639         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25640         assertSame(calibrator.getMeasurements(), measurements);
25641         assertTrue(calibrator.isCommonAxisUsed());
25642         assertSame(calibrator.getListener(), this);
25643         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25644         assertFalse(calibrator.isReady());
25645         assertFalse(calibrator.isRunning());
25646         assertNull(calibrator.getEstimatedBiases());
25647         assertFalse(calibrator.getEstimatedBiases(null));
25648         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25649         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25650         assertNull(calibrator.getEstimatedBiasFx());
25651         assertNull(calibrator.getEstimatedBiasFy());
25652         assertNull(calibrator.getEstimatedBiasFz());
25653         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25654         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25655         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25656         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25657         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25658         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25659         assertNull(calibrator.getEstimatedMa());
25660         assertNull(calibrator.getEstimatedSx());
25661         assertNull(calibrator.getEstimatedSy());
25662         assertNull(calibrator.getEstimatedSz());
25663         assertNull(calibrator.getEstimatedMxy());
25664         assertNull(calibrator.getEstimatedMxz());
25665         assertNull(calibrator.getEstimatedMyx());
25666         assertNull(calibrator.getEstimatedMyz());
25667         assertNull(calibrator.getEstimatedMzx());
25668         assertNull(calibrator.getEstimatedMzy());
25669         assertNull(calibrator.getEstimatedCovariance());
25670         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25671     }
25672 
25673     @Test
25674     public void testConstructor211() throws WrongSizeException {
25675         final Matrix ba = generateBa();
25676         final double[] bias = ba.getBuffer();
25677         final double biasX = ba.getElementAtIndex(0);
25678         final double biasY = ba.getElementAtIndex(1);
25679         final double biasZ = ba.getElementAtIndex(2);
25680 
25681         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25682         final double latitude = Math.toRadians(
25683                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25684         final double longitude = Math.toRadians(
25685                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25686         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25687         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25688         final NEDVelocity nedVelocity = new NEDVelocity();
25689         final ECEFPosition ecefPosition = new ECEFPosition();
25690         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25691         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25692                 ecefPosition, ecefVelocity);
25693 
25694         KnownPositionAccelerometerCalibrator calibrator =
25695                 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
25696 
25697         // check default values
25698         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25699         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25700         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25701         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25702         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25703         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25704         final Acceleration bx2 = new Acceleration(0.0,
25705                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25706         calibrator.getInitialBiasXAsAcceleration(bx2);
25707         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25708         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25709         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25710         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25711         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25712         final Acceleration by2 = new Acceleration(0.0,
25713                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25714         calibrator.getInitialBiasYAsAcceleration(by2);
25715         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25716         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25717         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25718         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25719         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25720         final Acceleration bz2 = new Acceleration(0.0,
25721                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25722         calibrator.getInitialBiasZAsAcceleration(bz2);
25723         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25724         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25725         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25726         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25727         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25728         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25729         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25730         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25731         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25732         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25733         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25734         final double[] bias1 = calibrator.getInitialBias();
25735         assertArrayEquals(bias1, bias, 0.0);
25736         final double[] bias2 = new double[3];
25737         calibrator.getInitialBias(bias2);
25738         assertArrayEquals(bias1, bias2, 0.0);
25739         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25740         assertEquals(b1, ba);
25741         final Matrix b2 = new Matrix(3, 1);
25742         calibrator.getInitialBiasAsMatrix(b2);
25743         assertEquals(b1, b2);
25744         final Matrix ma1 = calibrator.getInitialMa();
25745         assertEquals(ma1, new Matrix(3, 3));
25746         final Matrix ma2 = new Matrix(3, 3);
25747         calibrator.getInitialMa(ma2);
25748         assertEquals(ma1, ma2);
25749         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25750         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25751         final NEDPosition nedPosition1 = new NEDPosition();
25752         assertTrue(calibrator.getNedPosition(nedPosition1));
25753         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25754         assertNull(calibrator.getMeasurements());
25755         assertFalse(calibrator.isCommonAxisUsed());
25756         assertNull(calibrator.getListener());
25757         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25758         assertFalse(calibrator.isReady());
25759         assertFalse(calibrator.isRunning());
25760         assertNull(calibrator.getEstimatedBiases());
25761         assertFalse(calibrator.getEstimatedBiases(null));
25762         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25763         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25764         assertNull(calibrator.getEstimatedBiasFx());
25765         assertNull(calibrator.getEstimatedBiasFy());
25766         assertNull(calibrator.getEstimatedBiasFz());
25767         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25768         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25769         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25770         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25771         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25772         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25773         assertNull(calibrator.getEstimatedMa());
25774         assertNull(calibrator.getEstimatedSx());
25775         assertNull(calibrator.getEstimatedSy());
25776         assertNull(calibrator.getEstimatedSz());
25777         assertNull(calibrator.getEstimatedMxy());
25778         assertNull(calibrator.getEstimatedMxz());
25779         assertNull(calibrator.getEstimatedMyx());
25780         assertNull(calibrator.getEstimatedMyz());
25781         assertNull(calibrator.getEstimatedMzx());
25782         assertNull(calibrator.getEstimatedMzy());
25783         assertNull(calibrator.getEstimatedCovariance());
25784         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25785 
25786         // Force IllegalArgumentException
25787         calibrator = null;
25788         try {
25789             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
25790                     new double[1]);
25791             fail("IllegalArgumentException expected but not thrown");
25792         } catch (final IllegalArgumentException ignore) {
25793         }
25794         assertNull(calibrator);
25795     }
25796 
25797     @Test
25798     public void testConstructor212() throws WrongSizeException {
25799         final Matrix ba = generateBa();
25800         final double[] bias = ba.getBuffer();
25801         final double biasX = ba.getElementAtIndex(0);
25802         final double biasY = ba.getElementAtIndex(1);
25803         final double biasZ = ba.getElementAtIndex(2);
25804 
25805         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25806         final double latitude = Math.toRadians(
25807                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25808         final double longitude = Math.toRadians(
25809                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25810         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25811         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25812         final NEDVelocity nedVelocity = new NEDVelocity();
25813         final ECEFPosition ecefPosition = new ECEFPosition();
25814         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25815         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25816                 ecefPosition, ecefVelocity);
25817 
25818         KnownPositionAccelerometerCalibrator calibrator =
25819                 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
25820                         this);
25821 
25822         // check default values
25823         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25824         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25825         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25826         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25827         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25828         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25829         final Acceleration bx2 = new Acceleration(0.0,
25830                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25831         calibrator.getInitialBiasXAsAcceleration(bx2);
25832         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25833         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25834         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25835         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25836         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25837         final Acceleration by2 = new Acceleration(0.0,
25838                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25839         calibrator.getInitialBiasYAsAcceleration(by2);
25840         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25841         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25842         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25843         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25844         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25845         final Acceleration bz2 = new Acceleration(0.0,
25846                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25847         calibrator.getInitialBiasZAsAcceleration(bz2);
25848         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25849         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25850         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25851         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25852         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25853         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25854         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25855         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25856         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25857         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25858         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25859         final double[] bias1 = calibrator.getInitialBias();
25860         assertArrayEquals(bias1, bias, 0.0);
25861         final double[] bias2 = new double[3];
25862         calibrator.getInitialBias(bias2);
25863         assertArrayEquals(bias1, bias2, 0.0);
25864         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25865         assertEquals(b1, ba);
25866         final Matrix b2 = new Matrix(3, 1);
25867         calibrator.getInitialBiasAsMatrix(b2);
25868         assertEquals(b1, b2);
25869         final Matrix ma1 = calibrator.getInitialMa();
25870         assertEquals(ma1, new Matrix(3, 3));
25871         final Matrix ma2 = new Matrix(3, 3);
25872         calibrator.getInitialMa(ma2);
25873         assertEquals(ma1, ma2);
25874         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25875         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25876         final NEDPosition nedPosition1 = new NEDPosition();
25877         assertTrue(calibrator.getNedPosition(nedPosition1));
25878         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25879         assertNull(calibrator.getMeasurements());
25880         assertFalse(calibrator.isCommonAxisUsed());
25881         assertSame(calibrator.getListener(), this);
25882         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25883         assertFalse(calibrator.isReady());
25884         assertFalse(calibrator.isRunning());
25885         assertNull(calibrator.getEstimatedBiases());
25886         assertFalse(calibrator.getEstimatedBiases(null));
25887         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25888         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25889         assertNull(calibrator.getEstimatedBiasFx());
25890         assertNull(calibrator.getEstimatedBiasFy());
25891         assertNull(calibrator.getEstimatedBiasFz());
25892         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25893         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25894         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25895         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25896         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25897         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25898         assertNull(calibrator.getEstimatedMa());
25899         assertNull(calibrator.getEstimatedSx());
25900         assertNull(calibrator.getEstimatedSy());
25901         assertNull(calibrator.getEstimatedSz());
25902         assertNull(calibrator.getEstimatedMxy());
25903         assertNull(calibrator.getEstimatedMxz());
25904         assertNull(calibrator.getEstimatedMyx());
25905         assertNull(calibrator.getEstimatedMyz());
25906         assertNull(calibrator.getEstimatedMzx());
25907         assertNull(calibrator.getEstimatedMzy());
25908         assertNull(calibrator.getEstimatedCovariance());
25909         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25910 
25911         // Force IllegalArgumentException
25912         calibrator = null;
25913         try {
25914             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
25915                     new double[1], this);
25916             fail("IllegalArgumentException expected but not thrown");
25917         } catch (final IllegalArgumentException ignore) {
25918         }
25919         assertNull(calibrator);
25920     }
25921 
25922     @Test
25923     public void testConstructor213() throws WrongSizeException {
25924         final Collection<StandardDeviationBodyKinematics> measurements =
25925                 Collections.emptyList();
25926 
25927         final Matrix ba = generateBa();
25928         final double[] bias = ba.getBuffer();
25929         final double biasX = ba.getElementAtIndex(0);
25930         final double biasY = ba.getElementAtIndex(1);
25931         final double biasZ = ba.getElementAtIndex(2);
25932 
25933         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25934         final double latitude = Math.toRadians(
25935                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25936         final double longitude = Math.toRadians(
25937                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25938         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25939         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25940         final NEDVelocity nedVelocity = new NEDVelocity();
25941         final ECEFPosition ecefPosition = new ECEFPosition();
25942         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25943         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25944                 ecefPosition, ecefVelocity);
25945 
25946         KnownPositionAccelerometerCalibrator calibrator =
25947                 new KnownPositionAccelerometerCalibrator(nedPosition,
25948                         measurements, bias);
25949 
25950         // check default values
25951         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25952         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25953         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25954         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25955         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25956         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25957         final Acceleration bx2 = new Acceleration(0.0,
25958                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25959         calibrator.getInitialBiasXAsAcceleration(bx2);
25960         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25961         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25962         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25963         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25964         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25965         final Acceleration by2 = new Acceleration(0.0,
25966                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25967         calibrator.getInitialBiasYAsAcceleration(by2);
25968         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25969         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25970         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25971         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25972         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25973         final Acceleration bz2 = new Acceleration(0.0,
25974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25975         calibrator.getInitialBiasZAsAcceleration(bz2);
25976         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25977         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25978         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
25979         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
25980         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
25981         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25982         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25983         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25984         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25985         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25986         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25987         final double[] bias1 = calibrator.getInitialBias();
25988         assertArrayEquals(bias1, bias, 0.0);
25989         final double[] bias2 = new double[3];
25990         calibrator.getInitialBias(bias2);
25991         assertArrayEquals(bias1, bias2, 0.0);
25992         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25993         assertEquals(b1, ba);
25994         final Matrix b2 = new Matrix(3, 1);
25995         calibrator.getInitialBiasAsMatrix(b2);
25996         assertEquals(b1, b2);
25997         final Matrix ma1 = calibrator.getInitialMa();
25998         assertEquals(ma1, new Matrix(3, 3));
25999         final Matrix ma2 = new Matrix(3, 3);
26000         calibrator.getInitialMa(ma2);
26001         assertEquals(ma1, ma2);
26002         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26003         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26004         final NEDPosition nedPosition1 = new NEDPosition();
26005         assertTrue(calibrator.getNedPosition(nedPosition1));
26006         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26007         assertSame(calibrator.getMeasurements(), measurements);
26008         assertFalse(calibrator.isCommonAxisUsed());
26009         assertNull(calibrator.getListener());
26010         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26011         assertFalse(calibrator.isReady());
26012         assertFalse(calibrator.isRunning());
26013         assertNull(calibrator.getEstimatedBiases());
26014         assertFalse(calibrator.getEstimatedBiases(null));
26015         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26016         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26017         assertNull(calibrator.getEstimatedBiasFx());
26018         assertNull(calibrator.getEstimatedBiasFy());
26019         assertNull(calibrator.getEstimatedBiasFz());
26020         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26021         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26022         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26023         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26024         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26025         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26026         assertNull(calibrator.getEstimatedMa());
26027         assertNull(calibrator.getEstimatedSx());
26028         assertNull(calibrator.getEstimatedSy());
26029         assertNull(calibrator.getEstimatedSz());
26030         assertNull(calibrator.getEstimatedMxy());
26031         assertNull(calibrator.getEstimatedMxz());
26032         assertNull(calibrator.getEstimatedMyx());
26033         assertNull(calibrator.getEstimatedMyz());
26034         assertNull(calibrator.getEstimatedMzx());
26035         assertNull(calibrator.getEstimatedMzy());
26036         assertNull(calibrator.getEstimatedCovariance());
26037         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26038 
26039         // Force IllegalArgumentException
26040         calibrator = null;
26041         try {
26042             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26043                     measurements, new double[1]);
26044             fail("IllegalArgumentException expected but not thrown");
26045         } catch (final IllegalArgumentException ignore) {
26046         }
26047         assertNull(calibrator);
26048     }
26049 
26050     @Test
26051     public void testConstructor214() throws WrongSizeException {
26052         final Collection<StandardDeviationBodyKinematics> measurements =
26053                 Collections.emptyList();
26054 
26055         final Matrix ba = generateBa();
26056         final double[] bias = ba.getBuffer();
26057         final double biasX = ba.getElementAtIndex(0);
26058         final double biasY = ba.getElementAtIndex(1);
26059         final double biasZ = ba.getElementAtIndex(2);
26060 
26061         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26062         final double latitude = Math.toRadians(
26063                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26064         final double longitude = Math.toRadians(
26065                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26066         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26067         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26068         final NEDVelocity nedVelocity = new NEDVelocity();
26069         final ECEFPosition ecefPosition = new ECEFPosition();
26070         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26071         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26072                 ecefPosition, ecefVelocity);
26073 
26074         KnownPositionAccelerometerCalibrator calibrator =
26075                 new KnownPositionAccelerometerCalibrator(nedPosition,
26076                         measurements, bias, this);
26077 
26078         // check default values
26079         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26080         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26081         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26082         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26083         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26084         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26085         final Acceleration bx2 = new Acceleration(0.0,
26086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26087         calibrator.getInitialBiasXAsAcceleration(bx2);
26088         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26089         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26090         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26091         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26092         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26093         final Acceleration by2 = new Acceleration(0.0,
26094                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26095         calibrator.getInitialBiasYAsAcceleration(by2);
26096         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26097         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26098         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26099         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26100         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26101         final Acceleration bz2 = new Acceleration(0.0,
26102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26103         calibrator.getInitialBiasZAsAcceleration(bz2);
26104         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26105         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26106         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26107         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26108         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26109         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26110         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26111         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26112         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26113         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26114         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26115         final double[] bias1 = calibrator.getInitialBias();
26116         assertArrayEquals(bias1, bias, 0.0);
26117         final double[] bias2 = new double[3];
26118         calibrator.getInitialBias(bias2);
26119         assertArrayEquals(bias1, bias2, 0.0);
26120         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26121         assertEquals(b1, ba);
26122         final Matrix b2 = new Matrix(3, 1);
26123         calibrator.getInitialBiasAsMatrix(b2);
26124         assertEquals(b1, b2);
26125         final Matrix ma1 = calibrator.getInitialMa();
26126         assertEquals(ma1, new Matrix(3, 3));
26127         final Matrix ma2 = new Matrix(3, 3);
26128         calibrator.getInitialMa(ma2);
26129         assertEquals(ma1, ma2);
26130         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26131         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26132         final NEDPosition nedPosition1 = new NEDPosition();
26133         assertTrue(calibrator.getNedPosition(nedPosition1));
26134         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26135         assertSame(calibrator.getMeasurements(), measurements);
26136         assertFalse(calibrator.isCommonAxisUsed());
26137         assertSame(calibrator.getListener(), this);
26138         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26139         assertFalse(calibrator.isReady());
26140         assertFalse(calibrator.isRunning());
26141         assertNull(calibrator.getEstimatedBiases());
26142         assertFalse(calibrator.getEstimatedBiases(null));
26143         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26144         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26145         assertNull(calibrator.getEstimatedBiasFx());
26146         assertNull(calibrator.getEstimatedBiasFy());
26147         assertNull(calibrator.getEstimatedBiasFz());
26148         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26149         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26150         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26151         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26152         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26153         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26154         assertNull(calibrator.getEstimatedMa());
26155         assertNull(calibrator.getEstimatedSx());
26156         assertNull(calibrator.getEstimatedSy());
26157         assertNull(calibrator.getEstimatedSz());
26158         assertNull(calibrator.getEstimatedMxy());
26159         assertNull(calibrator.getEstimatedMxz());
26160         assertNull(calibrator.getEstimatedMyx());
26161         assertNull(calibrator.getEstimatedMyz());
26162         assertNull(calibrator.getEstimatedMzx());
26163         assertNull(calibrator.getEstimatedMzy());
26164         assertNull(calibrator.getEstimatedCovariance());
26165         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26166 
26167         // Force IllegalArgumentException
26168         calibrator = null;
26169         try {
26170             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26171                     measurements, new double[1], this);
26172             fail("IllegalArgumentException expected but not thrown");
26173         } catch (final IllegalArgumentException ignore) {
26174         }
26175         assertNull(calibrator);
26176     }
26177 
26178     @Test
26179     public void testConstructor215() throws WrongSizeException {
26180         final Matrix ba = generateBa();
26181         final double[] bias = ba.getBuffer();
26182         final double biasX = ba.getElementAtIndex(0);
26183         final double biasY = ba.getElementAtIndex(1);
26184         final double biasZ = ba.getElementAtIndex(2);
26185 
26186         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26187         final double latitude = Math.toRadians(
26188                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26189         final double longitude = Math.toRadians(
26190                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26191         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26192         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26193         final NEDVelocity nedVelocity = new NEDVelocity();
26194         final ECEFPosition ecefPosition = new ECEFPosition();
26195         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26196         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26197                 ecefPosition, ecefVelocity);
26198 
26199         KnownPositionAccelerometerCalibrator calibrator =
26200                 new KnownPositionAccelerometerCalibrator(nedPosition,
26201                         true, bias);
26202 
26203         // check default values
26204         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26205         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26206         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26207         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26208         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26209         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26210         final Acceleration bx2 = new Acceleration(0.0,
26211                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26212         calibrator.getInitialBiasXAsAcceleration(bx2);
26213         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26214         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26215         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26216         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26217         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26218         final Acceleration by2 = new Acceleration(0.0,
26219                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26220         calibrator.getInitialBiasYAsAcceleration(by2);
26221         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26222         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26223         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26224         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26225         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26226         final Acceleration bz2 = new Acceleration(0.0,
26227                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26228         calibrator.getInitialBiasZAsAcceleration(bz2);
26229         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26230         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26231         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26232         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26233         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26234         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26235         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26236         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26237         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26238         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26239         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26240         final double[] bias1 = calibrator.getInitialBias();
26241         assertArrayEquals(bias1, bias, 0.0);
26242         final double[] bias2 = new double[3];
26243         calibrator.getInitialBias(bias2);
26244         assertArrayEquals(bias1, bias2, 0.0);
26245         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26246         assertEquals(b1, ba);
26247         final Matrix b2 = new Matrix(3, 1);
26248         calibrator.getInitialBiasAsMatrix(b2);
26249         assertEquals(b1, b2);
26250         final Matrix ma1 = calibrator.getInitialMa();
26251         assertEquals(ma1, new Matrix(3, 3));
26252         final Matrix ma2 = new Matrix(3, 3);
26253         calibrator.getInitialMa(ma2);
26254         assertEquals(ma1, ma2);
26255         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26256         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26257         final NEDPosition nedPosition1 = new NEDPosition();
26258         assertTrue(calibrator.getNedPosition(nedPosition1));
26259         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26260         assertNull(calibrator.getMeasurements());
26261         assertTrue(calibrator.isCommonAxisUsed());
26262         assertNull(calibrator.getListener());
26263         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26264         assertFalse(calibrator.isReady());
26265         assertFalse(calibrator.isRunning());
26266         assertNull(calibrator.getEstimatedBiases());
26267         assertFalse(calibrator.getEstimatedBiases(null));
26268         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26269         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26270         assertNull(calibrator.getEstimatedBiasFx());
26271         assertNull(calibrator.getEstimatedBiasFy());
26272         assertNull(calibrator.getEstimatedBiasFz());
26273         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26274         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26275         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26276         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26277         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26278         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26279         assertNull(calibrator.getEstimatedMa());
26280         assertNull(calibrator.getEstimatedSx());
26281         assertNull(calibrator.getEstimatedSy());
26282         assertNull(calibrator.getEstimatedSz());
26283         assertNull(calibrator.getEstimatedMxy());
26284         assertNull(calibrator.getEstimatedMxz());
26285         assertNull(calibrator.getEstimatedMyx());
26286         assertNull(calibrator.getEstimatedMyz());
26287         assertNull(calibrator.getEstimatedMzx());
26288         assertNull(calibrator.getEstimatedMzy());
26289         assertNull(calibrator.getEstimatedCovariance());
26290         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26291 
26292         // Force IllegalArgumentException
26293         calibrator = null;
26294         try {
26295             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26296                     true, new double[1]);
26297             fail("IllegalArgumentException expected but not thrown");
26298         } catch (final IllegalArgumentException ignore) {
26299         }
26300         assertNull(calibrator);
26301     }
26302 
26303     @Test
26304     public void testConstructor216() throws WrongSizeException {
26305         final Matrix ba = generateBa();
26306         final double[] bias = ba.getBuffer();
26307         final double biasX = ba.getElementAtIndex(0);
26308         final double biasY = ba.getElementAtIndex(1);
26309         final double biasZ = ba.getElementAtIndex(2);
26310 
26311         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26312         final double latitude = Math.toRadians(
26313                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26314         final double longitude = Math.toRadians(
26315                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26316         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26317         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26318         final NEDVelocity nedVelocity = new NEDVelocity();
26319         final ECEFPosition ecefPosition = new ECEFPosition();
26320         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26321         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26322                 ecefPosition, ecefVelocity);
26323 
26324         KnownPositionAccelerometerCalibrator calibrator =
26325                 new KnownPositionAccelerometerCalibrator(nedPosition,
26326                         true, bias, this);
26327 
26328         // check default values
26329         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26330         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26331         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26332         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26333         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26334         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26335         final Acceleration bx2 = new Acceleration(0.0,
26336                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26337         calibrator.getInitialBiasXAsAcceleration(bx2);
26338         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26339         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26340         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26341         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26342         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26343         final Acceleration by2 = new Acceleration(0.0,
26344                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26345         calibrator.getInitialBiasYAsAcceleration(by2);
26346         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26347         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26348         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26349         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26350         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26351         final Acceleration bz2 = new Acceleration(0.0,
26352                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26353         calibrator.getInitialBiasZAsAcceleration(bz2);
26354         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26355         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26356         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26357         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26358         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26359         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26360         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26361         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26362         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26363         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26364         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26365         final double[] bias1 = calibrator.getInitialBias();
26366         assertArrayEquals(bias1, bias, 0.0);
26367         final double[] bias2 = new double[3];
26368         calibrator.getInitialBias(bias2);
26369         assertArrayEquals(bias1, bias2, 0.0);
26370         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26371         assertEquals(b1, ba);
26372         final Matrix b2 = new Matrix(3, 1);
26373         calibrator.getInitialBiasAsMatrix(b2);
26374         assertEquals(b1, b2);
26375         final Matrix ma1 = calibrator.getInitialMa();
26376         assertEquals(ma1, new Matrix(3, 3));
26377         final Matrix ma2 = new Matrix(3, 3);
26378         calibrator.getInitialMa(ma2);
26379         assertEquals(ma1, ma2);
26380         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26381         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26382         final NEDPosition nedPosition1 = new NEDPosition();
26383         assertTrue(calibrator.getNedPosition(nedPosition1));
26384         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26385         assertNull(calibrator.getMeasurements());
26386         assertTrue(calibrator.isCommonAxisUsed());
26387         assertSame(calibrator.getListener(), this);
26388         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26389         assertFalse(calibrator.isReady());
26390         assertFalse(calibrator.isRunning());
26391         assertNull(calibrator.getEstimatedBiases());
26392         assertFalse(calibrator.getEstimatedBiases(null));
26393         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26394         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26395         assertNull(calibrator.getEstimatedBiasFx());
26396         assertNull(calibrator.getEstimatedBiasFy());
26397         assertNull(calibrator.getEstimatedBiasFz());
26398         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26399         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26400         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26401         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26402         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26403         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26404         assertNull(calibrator.getEstimatedMa());
26405         assertNull(calibrator.getEstimatedSx());
26406         assertNull(calibrator.getEstimatedSy());
26407         assertNull(calibrator.getEstimatedSz());
26408         assertNull(calibrator.getEstimatedMxy());
26409         assertNull(calibrator.getEstimatedMxz());
26410         assertNull(calibrator.getEstimatedMyx());
26411         assertNull(calibrator.getEstimatedMyz());
26412         assertNull(calibrator.getEstimatedMzx());
26413         assertNull(calibrator.getEstimatedMzy());
26414         assertNull(calibrator.getEstimatedCovariance());
26415         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26416 
26417         // Force IllegalArgumentException
26418         calibrator = null;
26419         try {
26420             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26421                     true, new double[1], this);
26422             fail("IllegalArgumentException expected but not thrown");
26423         } catch (final IllegalArgumentException ignore) {
26424         }
26425         assertNull(calibrator);
26426     }
26427 
26428     @Test
26429     public void testConstructor217() throws WrongSizeException {
26430         final Collection<StandardDeviationBodyKinematics> measurements =
26431                 Collections.emptyList();
26432 
26433         final Matrix ba = generateBa();
26434         final double[] bias = ba.getBuffer();
26435         final double biasX = ba.getElementAtIndex(0);
26436         final double biasY = ba.getElementAtIndex(1);
26437         final double biasZ = ba.getElementAtIndex(2);
26438 
26439         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26440         final double latitude = Math.toRadians(
26441                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26442         final double longitude = Math.toRadians(
26443                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26444         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26445         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26446         final NEDVelocity nedVelocity = new NEDVelocity();
26447         final ECEFPosition ecefPosition = new ECEFPosition();
26448         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26449         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26450                 ecefPosition, ecefVelocity);
26451 
26452         KnownPositionAccelerometerCalibrator calibrator =
26453                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26454                         true, bias);
26455 
26456         // check default values
26457         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26458         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26459         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26460         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26461         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26462         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26463         final Acceleration bx2 = new Acceleration(0.0,
26464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26465         calibrator.getInitialBiasXAsAcceleration(bx2);
26466         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26467         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26468         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26469         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26470         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26471         final Acceleration by2 = new Acceleration(0.0,
26472                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26473         calibrator.getInitialBiasYAsAcceleration(by2);
26474         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26475         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26476         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26477         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26478         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26479         final Acceleration bz2 = new Acceleration(0.0,
26480                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26481         calibrator.getInitialBiasZAsAcceleration(bz2);
26482         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26483         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26484         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26485         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26486         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26487         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26488         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26489         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26490         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26491         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26492         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26493         final double[] bias1 = calibrator.getInitialBias();
26494         assertArrayEquals(bias1, bias, 0.0);
26495         final double[] bias2 = new double[3];
26496         calibrator.getInitialBias(bias2);
26497         assertArrayEquals(bias1, bias2, 0.0);
26498         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26499         assertEquals(b1, ba);
26500         final Matrix b2 = new Matrix(3, 1);
26501         calibrator.getInitialBiasAsMatrix(b2);
26502         assertEquals(b1, b2);
26503         final Matrix ma1 = calibrator.getInitialMa();
26504         assertEquals(ma1, new Matrix(3, 3));
26505         final Matrix ma2 = new Matrix(3, 3);
26506         calibrator.getInitialMa(ma2);
26507         assertEquals(ma1, ma2);
26508         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26509         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26510         final NEDPosition nedPosition1 = new NEDPosition();
26511         assertTrue(calibrator.getNedPosition(nedPosition1));
26512         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26513         assertSame(calibrator.getMeasurements(), measurements);
26514         assertTrue(calibrator.isCommonAxisUsed());
26515         assertNull(calibrator.getListener());
26516         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26517         assertFalse(calibrator.isReady());
26518         assertFalse(calibrator.isRunning());
26519         assertNull(calibrator.getEstimatedBiases());
26520         assertFalse(calibrator.getEstimatedBiases(null));
26521         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26522         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26523         assertNull(calibrator.getEstimatedBiasFx());
26524         assertNull(calibrator.getEstimatedBiasFy());
26525         assertNull(calibrator.getEstimatedBiasFz());
26526         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26527         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26528         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26529         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26530         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26531         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26532         assertNull(calibrator.getEstimatedMa());
26533         assertNull(calibrator.getEstimatedSx());
26534         assertNull(calibrator.getEstimatedSy());
26535         assertNull(calibrator.getEstimatedSz());
26536         assertNull(calibrator.getEstimatedMxy());
26537         assertNull(calibrator.getEstimatedMxz());
26538         assertNull(calibrator.getEstimatedMyx());
26539         assertNull(calibrator.getEstimatedMyz());
26540         assertNull(calibrator.getEstimatedMzx());
26541         assertNull(calibrator.getEstimatedMzy());
26542         assertNull(calibrator.getEstimatedCovariance());
26543         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26544 
26545         // Force IllegalArgumentException
26546         calibrator = null;
26547         try {
26548             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26549                     measurements, true, new double[1]);
26550             fail("IllegalArgumentException expected but not thrown");
26551         } catch (final IllegalArgumentException ignore) {
26552         }
26553         assertNull(calibrator);
26554     }
26555 
26556     @Test
26557     public void testConstructor218() throws WrongSizeException {
26558         final Collection<StandardDeviationBodyKinematics> measurements =
26559                 Collections.emptyList();
26560 
26561         final Matrix ba = generateBa();
26562         final double[] bias = ba.getBuffer();
26563         final double biasX = ba.getElementAtIndex(0);
26564         final double biasY = ba.getElementAtIndex(1);
26565         final double biasZ = ba.getElementAtIndex(2);
26566 
26567         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26568         final double latitude = Math.toRadians(
26569                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26570         final double longitude = Math.toRadians(
26571                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26572         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26573         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26574         final NEDVelocity nedVelocity = new NEDVelocity();
26575         final ECEFPosition ecefPosition = new ECEFPosition();
26576         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26577         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26578                 ecefPosition, ecefVelocity);
26579 
26580         KnownPositionAccelerometerCalibrator calibrator =
26581                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26582                         true, bias, this);
26583 
26584         // check default values
26585         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26586         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26587         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26588         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26589         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26590         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26591         final Acceleration bx2 = new Acceleration(0.0,
26592                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26593         calibrator.getInitialBiasXAsAcceleration(bx2);
26594         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26595         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26596         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26597         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26598         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26599         final Acceleration by2 = new Acceleration(0.0,
26600                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26601         calibrator.getInitialBiasYAsAcceleration(by2);
26602         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26603         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26604         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26605         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26606         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26607         final Acceleration bz2 = new Acceleration(0.0,
26608                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26609         calibrator.getInitialBiasZAsAcceleration(bz2);
26610         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26611         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26612         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26613         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26614         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26615         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26616         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26617         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26618         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26619         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26620         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26621         final double[] bias1 = calibrator.getInitialBias();
26622         assertArrayEquals(bias1, bias, 0.0);
26623         final double[] bias2 = new double[3];
26624         calibrator.getInitialBias(bias2);
26625         assertArrayEquals(bias1, bias2, 0.0);
26626         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26627         assertEquals(b1, ba);
26628         final Matrix b2 = new Matrix(3, 1);
26629         calibrator.getInitialBiasAsMatrix(b2);
26630         assertEquals(b1, b2);
26631         final Matrix ma1 = calibrator.getInitialMa();
26632         assertEquals(ma1, new Matrix(3, 3));
26633         final Matrix ma2 = new Matrix(3, 3);
26634         calibrator.getInitialMa(ma2);
26635         assertEquals(ma1, ma2);
26636         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26637         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26638         final NEDPosition nedPosition1 = new NEDPosition();
26639         assertTrue(calibrator.getNedPosition(nedPosition1));
26640         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26641         assertSame(calibrator.getMeasurements(), measurements);
26642         assertTrue(calibrator.isCommonAxisUsed());
26643         assertSame(calibrator.getListener(), this);
26644         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26645         assertFalse(calibrator.isReady());
26646         assertFalse(calibrator.isRunning());
26647         assertNull(calibrator.getEstimatedBiases());
26648         assertFalse(calibrator.getEstimatedBiases(null));
26649         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26650         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26651         assertNull(calibrator.getEstimatedBiasFx());
26652         assertNull(calibrator.getEstimatedBiasFy());
26653         assertNull(calibrator.getEstimatedBiasFz());
26654         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26655         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26656         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26657         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26658         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26659         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26660         assertNull(calibrator.getEstimatedMa());
26661         assertNull(calibrator.getEstimatedSx());
26662         assertNull(calibrator.getEstimatedSy());
26663         assertNull(calibrator.getEstimatedSz());
26664         assertNull(calibrator.getEstimatedMxy());
26665         assertNull(calibrator.getEstimatedMxz());
26666         assertNull(calibrator.getEstimatedMyx());
26667         assertNull(calibrator.getEstimatedMyz());
26668         assertNull(calibrator.getEstimatedMzx());
26669         assertNull(calibrator.getEstimatedMzy());
26670         assertNull(calibrator.getEstimatedCovariance());
26671         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26672 
26673         // Force IllegalArgumentException
26674         calibrator = null;
26675         try {
26676             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26677                     measurements, true, new double[1], this);
26678             fail("IllegalArgumentException expected but not thrown");
26679         } catch (final IllegalArgumentException ignore) {
26680         }
26681         assertNull(calibrator);
26682     }
26683 
26684     @Test
26685     public void testConstructor219() throws WrongSizeException {
26686         final Matrix ba = generateBa();
26687         final double[] bias = ba.getBuffer();
26688         final double biasX = ba.getElementAtIndex(0);
26689         final double biasY = ba.getElementAtIndex(1);
26690         final double biasZ = ba.getElementAtIndex(2);
26691 
26692         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26693         final double latitude = Math.toRadians(
26694                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26695         final double longitude = Math.toRadians(
26696                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26697         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26698         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26699         final NEDVelocity nedVelocity = new NEDVelocity();
26700         final ECEFPosition ecefPosition = new ECEFPosition();
26701         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26702         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26703                 ecefPosition, ecefVelocity);
26704 
26705         KnownPositionAccelerometerCalibrator calibrator =
26706                 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
26707 
26708         // check default values
26709         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26710         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26711         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26712         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26713         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26714         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26715         final Acceleration bx2 = new Acceleration(0.0,
26716                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26717         calibrator.getInitialBiasXAsAcceleration(bx2);
26718         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26719         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26720         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26721         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26722         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26723         final Acceleration by2 = new Acceleration(0.0,
26724                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26725         calibrator.getInitialBiasYAsAcceleration(by2);
26726         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26727         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26728         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26729         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26730         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26731         final Acceleration bz2 = new Acceleration(0.0,
26732                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26733         calibrator.getInitialBiasZAsAcceleration(bz2);
26734         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26735         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26736         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26737         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26738         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26739         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26740         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26741         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26742         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26743         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26744         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26745         final double[] bias1 = calibrator.getInitialBias();
26746         assertArrayEquals(bias1, bias, 0.0);
26747         final double[] bias2 = new double[3];
26748         calibrator.getInitialBias(bias2);
26749         assertArrayEquals(bias1, bias2, 0.0);
26750         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26751         assertEquals(b1, ba);
26752         final Matrix b2 = new Matrix(3, 1);
26753         calibrator.getInitialBiasAsMatrix(b2);
26754         assertEquals(b1, b2);
26755         final Matrix ma1 = calibrator.getInitialMa();
26756         assertEquals(ma1, new Matrix(3, 3));
26757         final Matrix ma2 = new Matrix(3, 3);
26758         calibrator.getInitialMa(ma2);
26759         assertEquals(ma1, ma2);
26760         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26761         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26762         final NEDPosition nedPosition1 = new NEDPosition();
26763         assertTrue(calibrator.getNedPosition(nedPosition1));
26764         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26765         assertNull(calibrator.getMeasurements());
26766         assertFalse(calibrator.isCommonAxisUsed());
26767         assertNull(calibrator.getListener());
26768         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26769         assertFalse(calibrator.isReady());
26770         assertFalse(calibrator.isRunning());
26771         assertNull(calibrator.getEstimatedBiases());
26772         assertFalse(calibrator.getEstimatedBiases(null));
26773         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26774         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26775         assertNull(calibrator.getEstimatedBiasFx());
26776         assertNull(calibrator.getEstimatedBiasFy());
26777         assertNull(calibrator.getEstimatedBiasFz());
26778         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26779         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26780         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26781         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26782         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26783         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26784         assertNull(calibrator.getEstimatedMa());
26785         assertNull(calibrator.getEstimatedSx());
26786         assertNull(calibrator.getEstimatedSy());
26787         assertNull(calibrator.getEstimatedSz());
26788         assertNull(calibrator.getEstimatedMxy());
26789         assertNull(calibrator.getEstimatedMxz());
26790         assertNull(calibrator.getEstimatedMyx());
26791         assertNull(calibrator.getEstimatedMyz());
26792         assertNull(calibrator.getEstimatedMzx());
26793         assertNull(calibrator.getEstimatedMzy());
26794         assertNull(calibrator.getEstimatedCovariance());
26795         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26796 
26797         // Force IllegalArgumentException
26798         calibrator = null;
26799         try {
26800             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26801                     new Matrix(1, 1));
26802             fail("IllegalArgumentException expected but not thrown");
26803         } catch (final IllegalArgumentException ignore) {
26804         }
26805         try {
26806             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26807                     new Matrix(1, 3));
26808             fail("IllegalArgumentException expected but not thrown");
26809         } catch (final IllegalArgumentException ignore) {
26810         }
26811         assertNull(calibrator);
26812     }
26813 
26814     @Test
26815     public void testConstructor220() throws WrongSizeException {
26816         final Matrix ba = generateBa();
26817         final double[] bias = ba.getBuffer();
26818         final double biasX = ba.getElementAtIndex(0);
26819         final double biasY = ba.getElementAtIndex(1);
26820         final double biasZ = ba.getElementAtIndex(2);
26821 
26822         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26823         final double latitude = Math.toRadians(
26824                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26825         final double longitude = Math.toRadians(
26826                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26827         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26828         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26829         final NEDVelocity nedVelocity = new NEDVelocity();
26830         final ECEFPosition ecefPosition = new ECEFPosition();
26831         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26832         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26833                 ecefPosition, ecefVelocity);
26834 
26835         KnownPositionAccelerometerCalibrator calibrator =
26836                 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
26837                         this);
26838 
26839         // check default values
26840         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26841         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26842         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26843         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26844         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26845         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26846         final Acceleration bx2 = new Acceleration(0.0,
26847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26848         calibrator.getInitialBiasXAsAcceleration(bx2);
26849         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26850         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26851         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26852         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26853         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26854         final Acceleration by2 = new Acceleration(0.0,
26855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26856         calibrator.getInitialBiasYAsAcceleration(by2);
26857         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26858         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26859         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26860         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26861         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26862         final Acceleration bz2 = new Acceleration(0.0,
26863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26864         calibrator.getInitialBiasZAsAcceleration(bz2);
26865         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26866         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26867         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
26868         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
26869         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
26870         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
26871         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
26872         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
26873         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
26874         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
26875         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
26876         final double[] bias1 = calibrator.getInitialBias();
26877         assertArrayEquals(bias1, bias, 0.0);
26878         final double[] bias2 = new double[3];
26879         calibrator.getInitialBias(bias2);
26880         assertArrayEquals(bias1, bias2, 0.0);
26881         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26882         assertEquals(b1, ba);
26883         final Matrix b2 = new Matrix(3, 1);
26884         calibrator.getInitialBiasAsMatrix(b2);
26885         assertEquals(b1, b2);
26886         final Matrix ma1 = calibrator.getInitialMa();
26887         assertEquals(ma1, new Matrix(3, 3));
26888         final Matrix ma2 = new Matrix(3, 3);
26889         calibrator.getInitialMa(ma2);
26890         assertEquals(ma1, ma2);
26891         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26892         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26893         final NEDPosition nedPosition1 = new NEDPosition();
26894         assertTrue(calibrator.getNedPosition(nedPosition1));
26895         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26896         assertNull(calibrator.getMeasurements());
26897         assertFalse(calibrator.isCommonAxisUsed());
26898         assertSame(calibrator.getListener(), this);
26899         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26900         assertFalse(calibrator.isReady());
26901         assertFalse(calibrator.isRunning());
26902         assertNull(calibrator.getEstimatedBiases());
26903         assertFalse(calibrator.getEstimatedBiases(null));
26904         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26905         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26906         assertNull(calibrator.getEstimatedBiasFx());
26907         assertNull(calibrator.getEstimatedBiasFy());
26908         assertNull(calibrator.getEstimatedBiasFz());
26909         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26910         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26911         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26912         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26913         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26914         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26915         assertNull(calibrator.getEstimatedMa());
26916         assertNull(calibrator.getEstimatedSx());
26917         assertNull(calibrator.getEstimatedSy());
26918         assertNull(calibrator.getEstimatedSz());
26919         assertNull(calibrator.getEstimatedMxy());
26920         assertNull(calibrator.getEstimatedMxz());
26921         assertNull(calibrator.getEstimatedMyx());
26922         assertNull(calibrator.getEstimatedMyz());
26923         assertNull(calibrator.getEstimatedMzx());
26924         assertNull(calibrator.getEstimatedMzy());
26925         assertNull(calibrator.getEstimatedCovariance());
26926         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26927 
26928         // Force IllegalArgumentException
26929         calibrator = null;
26930         try {
26931             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26932                     new Matrix(1, 1), this);
26933             fail("IllegalArgumentException expected but not thrown");
26934         } catch (final IllegalArgumentException ignore) {
26935         }
26936         try {
26937             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
26938                     new Matrix(1, 3), this);
26939             fail("IllegalArgumentException expected but not thrown");
26940         } catch (final IllegalArgumentException ignore) {
26941         }
26942         assertNull(calibrator);
26943     }
26944 
26945     @Test
26946     public void testConstructor221() throws WrongSizeException {
26947         final Collection<StandardDeviationBodyKinematics> measurements =
26948                 Collections.emptyList();
26949 
26950         final Matrix ba = generateBa();
26951         final double[] bias = ba.getBuffer();
26952         final double biasX = ba.getElementAtIndex(0);
26953         final double biasY = ba.getElementAtIndex(1);
26954         final double biasZ = ba.getElementAtIndex(2);
26955 
26956         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26957         final double latitude = Math.toRadians(
26958                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26959         final double longitude = Math.toRadians(
26960                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26961         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26962         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26963         final NEDVelocity nedVelocity = new NEDVelocity();
26964         final ECEFPosition ecefPosition = new ECEFPosition();
26965         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26966         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26967                 ecefPosition, ecefVelocity);
26968 
26969         KnownPositionAccelerometerCalibrator calibrator =
26970                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26971                         ba);
26972 
26973         // check default values
26974         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26975         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26976         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26977         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26978         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26979         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26980         final Acceleration bx2 = new Acceleration(0.0,
26981                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26982         calibrator.getInitialBiasXAsAcceleration(bx2);
26983         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26984         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26985         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26986         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26987         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26988         final Acceleration by2 = new Acceleration(0.0,
26989                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26990         calibrator.getInitialBiasYAsAcceleration(by2);
26991         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26992         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26993         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26994         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26995         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26996         final Acceleration bz2 = new Acceleration(0.0,
26997                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26998         calibrator.getInitialBiasZAsAcceleration(bz2);
26999         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27000         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27001         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27002         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27003         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27004         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27005         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27006         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27007         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27008         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27009         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27010         final double[] bias1 = calibrator.getInitialBias();
27011         assertArrayEquals(bias1, bias, 0.0);
27012         final double[] bias2 = new double[3];
27013         calibrator.getInitialBias(bias2);
27014         assertArrayEquals(bias1, bias2, 0.0);
27015         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27016         assertEquals(b1, ba);
27017         final Matrix b2 = new Matrix(3, 1);
27018         calibrator.getInitialBiasAsMatrix(b2);
27019         assertEquals(b1, b2);
27020         final Matrix ma1 = calibrator.getInitialMa();
27021         assertEquals(ma1, new Matrix(3, 3));
27022         final Matrix ma2 = new Matrix(3, 3);
27023         calibrator.getInitialMa(ma2);
27024         assertEquals(ma1, ma2);
27025         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27026         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27027         final NEDPosition nedPosition1 = new NEDPosition();
27028         assertTrue(calibrator.getNedPosition(nedPosition1));
27029         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27030         assertSame(calibrator.getMeasurements(), measurements);
27031         assertFalse(calibrator.isCommonAxisUsed());
27032         assertNull(calibrator.getListener());
27033         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27034         assertFalse(calibrator.isReady());
27035         assertFalse(calibrator.isRunning());
27036         assertNull(calibrator.getEstimatedBiases());
27037         assertFalse(calibrator.getEstimatedBiases(null));
27038         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27039         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27040         assertNull(calibrator.getEstimatedBiasFx());
27041         assertNull(calibrator.getEstimatedBiasFy());
27042         assertNull(calibrator.getEstimatedBiasFz());
27043         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27044         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27045         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27046         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27047         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27048         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27049         assertNull(calibrator.getEstimatedMa());
27050         assertNull(calibrator.getEstimatedSx());
27051         assertNull(calibrator.getEstimatedSy());
27052         assertNull(calibrator.getEstimatedSz());
27053         assertNull(calibrator.getEstimatedMxy());
27054         assertNull(calibrator.getEstimatedMxz());
27055         assertNull(calibrator.getEstimatedMyx());
27056         assertNull(calibrator.getEstimatedMyz());
27057         assertNull(calibrator.getEstimatedMzx());
27058         assertNull(calibrator.getEstimatedMzy());
27059         assertNull(calibrator.getEstimatedCovariance());
27060         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27061 
27062         // Force IllegalArgumentException
27063         calibrator = null;
27064         try {
27065             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27066                     measurements, new Matrix(1, 1));
27067             fail("IllegalArgumentException expected but not thrown");
27068         } catch (final IllegalArgumentException ignore) {
27069         }
27070         try {
27071             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27072                     measurements, new Matrix(1, 3));
27073             fail("IllegalArgumentException expected but not thrown");
27074         } catch (final IllegalArgumentException ignore) {
27075         }
27076         assertNull(calibrator);
27077     }
27078 
27079     @Test
27080     public void testConstructor222() throws WrongSizeException {
27081         final Collection<StandardDeviationBodyKinematics> measurements =
27082                 Collections.emptyList();
27083 
27084         final Matrix ba = generateBa();
27085         final double[] bias = ba.getBuffer();
27086         final double biasX = ba.getElementAtIndex(0);
27087         final double biasY = ba.getElementAtIndex(1);
27088         final double biasZ = ba.getElementAtIndex(2);
27089 
27090         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27091         final double latitude = Math.toRadians(
27092                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27093         final double longitude = Math.toRadians(
27094                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27095         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27096         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27097         final NEDVelocity nedVelocity = new NEDVelocity();
27098         final ECEFPosition ecefPosition = new ECEFPosition();
27099         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27100         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27101                 ecefPosition, ecefVelocity);
27102 
27103         KnownPositionAccelerometerCalibrator calibrator =
27104                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27105                         ba, this);
27106 
27107         // check default values
27108         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27109         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27110         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27111         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27112         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27113         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27114         final Acceleration bx2 = new Acceleration(0.0,
27115                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27116         calibrator.getInitialBiasXAsAcceleration(bx2);
27117         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27118         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27119         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27120         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27121         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27122         final Acceleration by2 = new Acceleration(0.0,
27123                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27124         calibrator.getInitialBiasYAsAcceleration(by2);
27125         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27126         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27127         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27128         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27129         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27130         final Acceleration bz2 = new Acceleration(0.0,
27131                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27132         calibrator.getInitialBiasZAsAcceleration(bz2);
27133         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27134         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27135         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27136         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27137         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27138         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27139         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27140         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27141         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27142         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27143         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27144         final double[] bias1 = calibrator.getInitialBias();
27145         assertArrayEquals(bias1, bias, 0.0);
27146         final double[] bias2 = new double[3];
27147         calibrator.getInitialBias(bias2);
27148         assertArrayEquals(bias1, bias2, 0.0);
27149         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27150         assertEquals(b1, ba);
27151         final Matrix b2 = new Matrix(3, 1);
27152         calibrator.getInitialBiasAsMatrix(b2);
27153         assertEquals(b1, b2);
27154         final Matrix ma1 = calibrator.getInitialMa();
27155         assertEquals(ma1, new Matrix(3, 3));
27156         final Matrix ma2 = new Matrix(3, 3);
27157         calibrator.getInitialMa(ma2);
27158         assertEquals(ma1, ma2);
27159         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27160         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27161         final NEDPosition nedPosition1 = new NEDPosition();
27162         assertTrue(calibrator.getNedPosition(nedPosition1));
27163         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27164         assertSame(calibrator.getMeasurements(), measurements);
27165         assertFalse(calibrator.isCommonAxisUsed());
27166         assertSame(calibrator.getListener(), this);
27167         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27168         assertFalse(calibrator.isReady());
27169         assertFalse(calibrator.isRunning());
27170         assertNull(calibrator.getEstimatedBiases());
27171         assertFalse(calibrator.getEstimatedBiases(null));
27172         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27173         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27174         assertNull(calibrator.getEstimatedBiasFx());
27175         assertNull(calibrator.getEstimatedBiasFy());
27176         assertNull(calibrator.getEstimatedBiasFz());
27177         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27178         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27179         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27180         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27181         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27182         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27183         assertNull(calibrator.getEstimatedMa());
27184         assertNull(calibrator.getEstimatedSx());
27185         assertNull(calibrator.getEstimatedSy());
27186         assertNull(calibrator.getEstimatedSz());
27187         assertNull(calibrator.getEstimatedMxy());
27188         assertNull(calibrator.getEstimatedMxz());
27189         assertNull(calibrator.getEstimatedMyx());
27190         assertNull(calibrator.getEstimatedMyz());
27191         assertNull(calibrator.getEstimatedMzx());
27192         assertNull(calibrator.getEstimatedMzy());
27193         assertNull(calibrator.getEstimatedCovariance());
27194         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27195 
27196         // Force IllegalArgumentException
27197         calibrator = null;
27198         try {
27199             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27200                     measurements, new Matrix(1, 1), this);
27201             fail("IllegalArgumentException expected but not thrown");
27202         } catch (final IllegalArgumentException ignore) {
27203         }
27204         try {
27205             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27206                     measurements, new Matrix(1, 3), this);
27207             fail("IllegalArgumentException expected but not thrown");
27208         } catch (final IllegalArgumentException ignore) {
27209         }
27210         assertNull(calibrator);
27211     }
27212 
27213     @Test
27214     public void testConstructor223() throws WrongSizeException {
27215         final Matrix ba = generateBa();
27216         final double[] bias = ba.getBuffer();
27217         final double biasX = ba.getElementAtIndex(0);
27218         final double biasY = ba.getElementAtIndex(1);
27219         final double biasZ = ba.getElementAtIndex(2);
27220 
27221         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27222         final double latitude = Math.toRadians(
27223                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27224         final double longitude = Math.toRadians(
27225                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27226         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27227         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27228         final NEDVelocity nedVelocity = new NEDVelocity();
27229         final ECEFPosition ecefPosition = new ECEFPosition();
27230         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27231         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27232                 ecefPosition, ecefVelocity);
27233 
27234         KnownPositionAccelerometerCalibrator calibrator =
27235                 new KnownPositionAccelerometerCalibrator(nedPosition,
27236                         true, ba);
27237 
27238         // check default values
27239         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27240         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27241         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27242         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27243         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27244         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27245         final Acceleration bx2 = new Acceleration(0.0,
27246                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27247         calibrator.getInitialBiasXAsAcceleration(bx2);
27248         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27249         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27250         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27251         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27252         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27253         final Acceleration by2 = new Acceleration(0.0,
27254                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27255         calibrator.getInitialBiasYAsAcceleration(by2);
27256         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27257         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27258         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27259         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27260         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27261         final Acceleration bz2 = new Acceleration(0.0,
27262                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27263         calibrator.getInitialBiasZAsAcceleration(bz2);
27264         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27265         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27266         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27267         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27268         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27269         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27270         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27271         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27272         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27273         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27274         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27275         final double[] bias1 = calibrator.getInitialBias();
27276         assertArrayEquals(bias1, bias, 0.0);
27277         final double[] bias2 = new double[3];
27278         calibrator.getInitialBias(bias2);
27279         assertArrayEquals(bias1, bias2, 0.0);
27280         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27281         assertEquals(b1, ba);
27282         final Matrix b2 = new Matrix(3, 1);
27283         calibrator.getInitialBiasAsMatrix(b2);
27284         assertEquals(b1, b2);
27285         final Matrix ma1 = calibrator.getInitialMa();
27286         assertEquals(ma1, new Matrix(3, 3));
27287         final Matrix ma2 = new Matrix(3, 3);
27288         calibrator.getInitialMa(ma2);
27289         assertEquals(ma1, ma2);
27290         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27291         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27292         final NEDPosition nedPosition1 = new NEDPosition();
27293         assertTrue(calibrator.getNedPosition(nedPosition1));
27294         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27295         assertNull(calibrator.getMeasurements());
27296         assertTrue(calibrator.isCommonAxisUsed());
27297         assertNull(calibrator.getListener());
27298         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27299         assertFalse(calibrator.isReady());
27300         assertFalse(calibrator.isRunning());
27301         assertNull(calibrator.getEstimatedBiases());
27302         assertFalse(calibrator.getEstimatedBiases(null));
27303         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27304         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27305         assertNull(calibrator.getEstimatedBiasFx());
27306         assertNull(calibrator.getEstimatedBiasFy());
27307         assertNull(calibrator.getEstimatedBiasFz());
27308         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27309         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27310         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27311         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27312         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27313         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27314         assertNull(calibrator.getEstimatedMa());
27315         assertNull(calibrator.getEstimatedSx());
27316         assertNull(calibrator.getEstimatedSy());
27317         assertNull(calibrator.getEstimatedSz());
27318         assertNull(calibrator.getEstimatedMxy());
27319         assertNull(calibrator.getEstimatedMxz());
27320         assertNull(calibrator.getEstimatedMyx());
27321         assertNull(calibrator.getEstimatedMyz());
27322         assertNull(calibrator.getEstimatedMzx());
27323         assertNull(calibrator.getEstimatedMzy());
27324         assertNull(calibrator.getEstimatedCovariance());
27325         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27326 
27327         // Force IllegalArgumentException
27328         calibrator = null;
27329         try {
27330             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27331                     true, new Matrix(1, 1));
27332             fail("IllegalArgumentException expected but not thrown");
27333         } catch (final IllegalArgumentException ignore) {
27334         }
27335         try {
27336             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27337                     true, new Matrix(1, 3));
27338             fail("IllegalArgumentException expected but not thrown");
27339         } catch (final IllegalArgumentException ignore) {
27340         }
27341         assertNull(calibrator);
27342     }
27343 
27344     @Test
27345     public void testConstructor224() throws WrongSizeException {
27346         final Matrix ba = generateBa();
27347         final double[] bias = ba.getBuffer();
27348         final double biasX = ba.getElementAtIndex(0);
27349         final double biasY = ba.getElementAtIndex(1);
27350         final double biasZ = ba.getElementAtIndex(2);
27351 
27352         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27353         final double latitude = Math.toRadians(
27354                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27355         final double longitude = Math.toRadians(
27356                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27357         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27358         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27359         final NEDVelocity nedVelocity = new NEDVelocity();
27360         final ECEFPosition ecefPosition = new ECEFPosition();
27361         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27362         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27363                 ecefPosition, ecefVelocity);
27364 
27365         KnownPositionAccelerometerCalibrator calibrator =
27366                 new KnownPositionAccelerometerCalibrator(nedPosition,
27367                         true, ba, this);
27368 
27369         // check default values
27370         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27371         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27372         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27373         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27374         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27375         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27376         final Acceleration bx2 = new Acceleration(0.0,
27377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27378         calibrator.getInitialBiasXAsAcceleration(bx2);
27379         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27380         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27381         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27382         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27383         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27384         final Acceleration by2 = new Acceleration(0.0,
27385                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27386         calibrator.getInitialBiasYAsAcceleration(by2);
27387         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27388         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27389         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27390         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27391         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27392         final Acceleration bz2 = new Acceleration(0.0,
27393                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27394         calibrator.getInitialBiasZAsAcceleration(bz2);
27395         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27396         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27397         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27398         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27399         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27400         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27401         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27402         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27403         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27404         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27405         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27406         final double[] bias1 = calibrator.getInitialBias();
27407         assertArrayEquals(bias1, bias, 0.0);
27408         final double[] bias2 = new double[3];
27409         calibrator.getInitialBias(bias2);
27410         assertArrayEquals(bias1, bias2, 0.0);
27411         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27412         assertEquals(b1, ba);
27413         final Matrix b2 = new Matrix(3, 1);
27414         calibrator.getInitialBiasAsMatrix(b2);
27415         assertEquals(b1, b2);
27416         final Matrix ma1 = calibrator.getInitialMa();
27417         assertEquals(ma1, new Matrix(3, 3));
27418         final Matrix ma2 = new Matrix(3, 3);
27419         calibrator.getInitialMa(ma2);
27420         assertEquals(ma1, ma2);
27421         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27422         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27423         final NEDPosition nedPosition1 = new NEDPosition();
27424         assertTrue(calibrator.getNedPosition(nedPosition1));
27425         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27426         assertNull(calibrator.getMeasurements());
27427         assertTrue(calibrator.isCommonAxisUsed());
27428         assertSame(calibrator.getListener(), this);
27429         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27430         assertFalse(calibrator.isReady());
27431         assertFalse(calibrator.isRunning());
27432         assertNull(calibrator.getEstimatedBiases());
27433         assertFalse(calibrator.getEstimatedBiases(null));
27434         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27435         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27436         assertNull(calibrator.getEstimatedBiasFx());
27437         assertNull(calibrator.getEstimatedBiasFy());
27438         assertNull(calibrator.getEstimatedBiasFz());
27439         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27440         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27441         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27442         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27443         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27444         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27445         assertNull(calibrator.getEstimatedMa());
27446         assertNull(calibrator.getEstimatedSx());
27447         assertNull(calibrator.getEstimatedSy());
27448         assertNull(calibrator.getEstimatedSz());
27449         assertNull(calibrator.getEstimatedMxy());
27450         assertNull(calibrator.getEstimatedMxz());
27451         assertNull(calibrator.getEstimatedMyx());
27452         assertNull(calibrator.getEstimatedMyz());
27453         assertNull(calibrator.getEstimatedMzx());
27454         assertNull(calibrator.getEstimatedMzy());
27455         assertNull(calibrator.getEstimatedCovariance());
27456         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27457 
27458         // Force IllegalArgumentException
27459         calibrator = null;
27460         try {
27461             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27462                     true, new Matrix(1, 1),
27463                     this);
27464             fail("IllegalArgumentException expected but not thrown");
27465         } catch (final IllegalArgumentException ignore) {
27466         }
27467         try {
27468             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27469                     true, new Matrix(1, 3),
27470                     this);
27471             fail("IllegalArgumentException expected but not thrown");
27472         } catch (final IllegalArgumentException ignore) {
27473         }
27474         assertNull(calibrator);
27475     }
27476 
27477     @Test
27478     public void testConstructor225() throws WrongSizeException {
27479         final Collection<StandardDeviationBodyKinematics> measurements =
27480                 Collections.emptyList();
27481 
27482         final Matrix ba = generateBa();
27483         final double[] bias = ba.getBuffer();
27484         final double biasX = ba.getElementAtIndex(0);
27485         final double biasY = ba.getElementAtIndex(1);
27486         final double biasZ = ba.getElementAtIndex(2);
27487 
27488         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27489         final double latitude = Math.toRadians(
27490                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27491         final double longitude = Math.toRadians(
27492                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27493         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27494         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27495         final NEDVelocity nedVelocity = new NEDVelocity();
27496         final ECEFPosition ecefPosition = new ECEFPosition();
27497         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27498         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27499                 ecefPosition, ecefVelocity);
27500 
27501         KnownPositionAccelerometerCalibrator calibrator =
27502                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27503                         true, ba);
27504 
27505         // check default values
27506         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27507         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27508         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27509         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27510         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27511         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27512         final Acceleration bx2 = new Acceleration(0.0,
27513                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27514         calibrator.getInitialBiasXAsAcceleration(bx2);
27515         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27516         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27517         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27518         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27519         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27520         final Acceleration by2 = new Acceleration(0.0,
27521                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27522         calibrator.getInitialBiasYAsAcceleration(by2);
27523         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27524         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27525         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27526         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27527         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27528         final Acceleration bz2 = new Acceleration(0.0,
27529                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27530         calibrator.getInitialBiasZAsAcceleration(bz2);
27531         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27532         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27533         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27534         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27535         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27536         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27537         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27538         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27539         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27540         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27541         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27542         final double[] bias1 = calibrator.getInitialBias();
27543         assertArrayEquals(bias1, bias, 0.0);
27544         final double[] bias2 = new double[3];
27545         calibrator.getInitialBias(bias2);
27546         assertArrayEquals(bias1, bias2, 0.0);
27547         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27548         assertEquals(b1, ba);
27549         final Matrix b2 = new Matrix(3, 1);
27550         calibrator.getInitialBiasAsMatrix(b2);
27551         assertEquals(b1, b2);
27552         final Matrix ma1 = calibrator.getInitialMa();
27553         assertEquals(ma1, new Matrix(3, 3));
27554         final Matrix ma2 = new Matrix(3, 3);
27555         calibrator.getInitialMa(ma2);
27556         assertEquals(ma1, ma2);
27557         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27558         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27559         final NEDPosition nedPosition1 = new NEDPosition();
27560         assertTrue(calibrator.getNedPosition(nedPosition1));
27561         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27562         assertSame(calibrator.getMeasurements(), measurements);
27563         assertTrue(calibrator.isCommonAxisUsed());
27564         assertNull(calibrator.getListener());
27565         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27566         assertFalse(calibrator.isReady());
27567         assertFalse(calibrator.isRunning());
27568         assertNull(calibrator.getEstimatedBiases());
27569         assertFalse(calibrator.getEstimatedBiases(null));
27570         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27571         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27572         assertNull(calibrator.getEstimatedBiasFx());
27573         assertNull(calibrator.getEstimatedBiasFy());
27574         assertNull(calibrator.getEstimatedBiasFz());
27575         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27576         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27577         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27578         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27579         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27580         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27581         assertNull(calibrator.getEstimatedMa());
27582         assertNull(calibrator.getEstimatedSx());
27583         assertNull(calibrator.getEstimatedSy());
27584         assertNull(calibrator.getEstimatedSz());
27585         assertNull(calibrator.getEstimatedMxy());
27586         assertNull(calibrator.getEstimatedMxz());
27587         assertNull(calibrator.getEstimatedMyx());
27588         assertNull(calibrator.getEstimatedMyz());
27589         assertNull(calibrator.getEstimatedMzx());
27590         assertNull(calibrator.getEstimatedMzy());
27591         assertNull(calibrator.getEstimatedCovariance());
27592         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27593 
27594         // Force IllegalArgumentException
27595         calibrator = null;
27596         try {
27597             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27598                     measurements, true,
27599                     new Matrix(1, 1));
27600             fail("IllegalArgumentException expected but not thrown");
27601         } catch (final IllegalArgumentException ignore) {
27602         }
27603         try {
27604             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27605                     measurements,true,
27606                     new Matrix(1, 3));
27607             fail("IllegalArgumentException expected but not thrown");
27608         } catch (final IllegalArgumentException ignore) {
27609         }
27610         assertNull(calibrator);
27611     }
27612 
27613     @Test
27614     public void testConstructor226() throws WrongSizeException {
27615         final Collection<StandardDeviationBodyKinematics> measurements =
27616                 Collections.emptyList();
27617 
27618         final Matrix ba = generateBa();
27619         final double[] bias = ba.getBuffer();
27620         final double biasX = ba.getElementAtIndex(0);
27621         final double biasY = ba.getElementAtIndex(1);
27622         final double biasZ = ba.getElementAtIndex(2);
27623 
27624         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27625         final double latitude = Math.toRadians(
27626                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27627         final double longitude = Math.toRadians(
27628                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27629         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27630         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27631         final NEDVelocity nedVelocity = new NEDVelocity();
27632         final ECEFPosition ecefPosition = new ECEFPosition();
27633         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27634         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27635                 ecefPosition, ecefVelocity);
27636 
27637         KnownPositionAccelerometerCalibrator calibrator =
27638                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27639                         true, ba, this);
27640 
27641         // check default values
27642         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27643         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27644         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27645         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27646         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27647         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27648         final Acceleration bx2 = new Acceleration(0.0,
27649                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27650         calibrator.getInitialBiasXAsAcceleration(bx2);
27651         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27652         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27653         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27654         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27655         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27656         final Acceleration by2 = new Acceleration(0.0,
27657                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27658         calibrator.getInitialBiasYAsAcceleration(by2);
27659         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27660         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27661         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27662         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27663         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27664         final Acceleration bz2 = new Acceleration(0.0,
27665                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27666         calibrator.getInitialBiasZAsAcceleration(bz2);
27667         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27668         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27669         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27670         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27671         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27672         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27673         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27674         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27675         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27676         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27677         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27678         final double[] bias1 = calibrator.getInitialBias();
27679         assertArrayEquals(bias1, bias, 0.0);
27680         final double[] bias2 = new double[3];
27681         calibrator.getInitialBias(bias2);
27682         assertArrayEquals(bias1, bias2, 0.0);
27683         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27684         assertEquals(b1, ba);
27685         final Matrix b2 = new Matrix(3, 1);
27686         calibrator.getInitialBiasAsMatrix(b2);
27687         assertEquals(b1, b2);
27688         final Matrix ma1 = calibrator.getInitialMa();
27689         assertEquals(ma1, new Matrix(3, 3));
27690         final Matrix ma2 = new Matrix(3, 3);
27691         calibrator.getInitialMa(ma2);
27692         assertEquals(ma1, ma2);
27693         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27694         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27695         final NEDPosition nedPosition1 = new NEDPosition();
27696         assertTrue(calibrator.getNedPosition(nedPosition1));
27697         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27698         assertSame(calibrator.getMeasurements(), measurements);
27699         assertTrue(calibrator.isCommonAxisUsed());
27700         assertSame(calibrator.getListener(), this);
27701         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27702         assertFalse(calibrator.isReady());
27703         assertFalse(calibrator.isRunning());
27704         assertNull(calibrator.getEstimatedBiases());
27705         assertFalse(calibrator.getEstimatedBiases(null));
27706         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27707         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27708         assertNull(calibrator.getEstimatedBiasFx());
27709         assertNull(calibrator.getEstimatedBiasFy());
27710         assertNull(calibrator.getEstimatedBiasFz());
27711         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27712         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27713         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27714         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27715         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27716         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27717         assertNull(calibrator.getEstimatedMa());
27718         assertNull(calibrator.getEstimatedSx());
27719         assertNull(calibrator.getEstimatedSy());
27720         assertNull(calibrator.getEstimatedSz());
27721         assertNull(calibrator.getEstimatedMxy());
27722         assertNull(calibrator.getEstimatedMxz());
27723         assertNull(calibrator.getEstimatedMyx());
27724         assertNull(calibrator.getEstimatedMyz());
27725         assertNull(calibrator.getEstimatedMzx());
27726         assertNull(calibrator.getEstimatedMzy());
27727         assertNull(calibrator.getEstimatedCovariance());
27728         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27729 
27730         // Force IllegalArgumentException
27731         calibrator = null;
27732         try {
27733             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27734                     measurements, true,
27735                     new Matrix(1, 1), this);
27736             fail("IllegalArgumentException expected but not thrown");
27737         } catch (final IllegalArgumentException ignore) {
27738         }
27739         try {
27740             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27741                     measurements,true,
27742                     new Matrix(1, 3), this);
27743             fail("IllegalArgumentException expected but not thrown");
27744         } catch (final IllegalArgumentException ignore) {
27745         }
27746         assertNull(calibrator);
27747     }
27748 
27749     @Test
27750     public void testConstructor227() throws WrongSizeException {
27751         final Matrix ba = generateBa();
27752         final double[] bias = ba.getBuffer();
27753         final double biasX = ba.getElementAtIndex(0);
27754         final double biasY = ba.getElementAtIndex(1);
27755         final double biasZ = ba.getElementAtIndex(2);
27756 
27757         final Matrix ma = generateMaCommonAxis();
27758         final double sx = ma.getElementAt(0, 0);
27759         final double sy = ma.getElementAt(1, 1);
27760         final double sz = ma.getElementAt(2, 2);
27761         final double mxy = ma.getElementAt(0, 1);
27762         final double mxz = ma.getElementAt(0, 2);
27763         final double myx = ma.getElementAt(1, 0);
27764         final double myz = ma.getElementAt(1, 2);
27765         final double mzx = ma.getElementAt(2, 0);
27766         final double mzy = ma.getElementAt(2, 1);
27767 
27768         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27769         final double latitude = Math.toRadians(
27770                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27771         final double longitude = Math.toRadians(
27772                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27773         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27774         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27775         final NEDVelocity nedVelocity = new NEDVelocity();
27776         final ECEFPosition ecefPosition = new ECEFPosition();
27777         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27778         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27779                 ecefPosition, ecefVelocity);
27780 
27781         KnownPositionAccelerometerCalibrator calibrator =
27782                 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
27783 
27784         // check default values
27785         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27786         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27787         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27788         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27789         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27790         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27791         final Acceleration bx2 = new Acceleration(0.0,
27792                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27793         calibrator.getInitialBiasXAsAcceleration(bx2);
27794         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27795         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27796         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27797         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27798         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27799         final Acceleration by2 = new Acceleration(0.0,
27800                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27801         calibrator.getInitialBiasYAsAcceleration(by2);
27802         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27803         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27804         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27805         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27806         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27807         final Acceleration bz2 = new Acceleration(0.0,
27808                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27809         calibrator.getInitialBiasZAsAcceleration(bz2);
27810         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27811         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27812         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27813         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27814         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27815         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27816         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27817         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27818         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27819         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27820         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27821         final double[] bias1 = calibrator.getInitialBias();
27822         assertArrayEquals(bias1, bias, 0.0);
27823         final double[] bias2 = new double[3];
27824         calibrator.getInitialBias(bias2);
27825         assertArrayEquals(bias1, bias2, 0.0);
27826         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27827         assertEquals(b1, ba);
27828         final Matrix b2 = new Matrix(3, 1);
27829         calibrator.getInitialBiasAsMatrix(b2);
27830         assertEquals(b1, b2);
27831         final Matrix ma1 = new Matrix(3, 3);
27832         ma1.setSubmatrix(0, 0,
27833                 2, 2,
27834                 new double[]{ sx, myx, mzx,
27835                         mxy, sy, mzy,
27836                         mxz, myz, sz});
27837         assertEquals(calibrator.getInitialMa(), ma1);
27838         final Matrix ma2 = new Matrix(3, 3);
27839         calibrator.getInitialMa(ma2);
27840         assertEquals(ma1, ma2);
27841         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27842         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27843         final NEDPosition nedPosition1 = new NEDPosition();
27844         assertTrue(calibrator.getNedPosition(nedPosition1));
27845         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27846         assertNull(calibrator.getMeasurements());
27847         assertFalse(calibrator.isCommonAxisUsed());
27848         assertNull(calibrator.getListener());
27849         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27850         assertFalse(calibrator.isReady());
27851         assertFalse(calibrator.isRunning());
27852         assertNull(calibrator.getEstimatedBiases());
27853         assertFalse(calibrator.getEstimatedBiases(null));
27854         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27855         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27856         assertNull(calibrator.getEstimatedBiasFx());
27857         assertNull(calibrator.getEstimatedBiasFy());
27858         assertNull(calibrator.getEstimatedBiasFz());
27859         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27860         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27861         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27862         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27863         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27864         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27865         assertNull(calibrator.getEstimatedMa());
27866         assertNull(calibrator.getEstimatedSx());
27867         assertNull(calibrator.getEstimatedSy());
27868         assertNull(calibrator.getEstimatedSz());
27869         assertNull(calibrator.getEstimatedMxy());
27870         assertNull(calibrator.getEstimatedMxz());
27871         assertNull(calibrator.getEstimatedMyx());
27872         assertNull(calibrator.getEstimatedMyz());
27873         assertNull(calibrator.getEstimatedMzx());
27874         assertNull(calibrator.getEstimatedMzy());
27875         assertNull(calibrator.getEstimatedCovariance());
27876         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27877 
27878         // Force IllegalArgumentException
27879         calibrator = null;
27880         try {
27881             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27882                     new Matrix(1, 1), ma);
27883             fail("IllegalArgumentException expected but not thrown");
27884         } catch (final IllegalArgumentException ignore) {
27885         }
27886         try {
27887             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27888                     new Matrix(1, 3), ma);
27889             fail("IllegalArgumentException expected but not thrown");
27890         } catch (final IllegalArgumentException ignore) {
27891         }
27892         try {
27893             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27894                     ba, new Matrix(1, 3));
27895             fail("IllegalArgumentException expected but not thrown");
27896         } catch (final IllegalArgumentException ignore) {
27897         }
27898         try {
27899             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27900                     ba, new Matrix(3, 1));
27901             fail("IllegalArgumentException expected but not thrown");
27902         } catch (final IllegalArgumentException ignore) {
27903         }
27904         assertNull(calibrator);
27905     }
27906 
27907     @Test
27908     public void testConstructor228() throws WrongSizeException {
27909         final Matrix ba = generateBa();
27910         final double[] bias = ba.getBuffer();
27911         final double biasX = ba.getElementAtIndex(0);
27912         final double biasY = ba.getElementAtIndex(1);
27913         final double biasZ = ba.getElementAtIndex(2);
27914 
27915         final Matrix ma = generateMaCommonAxis();
27916         final double sx = ma.getElementAt(0, 0);
27917         final double sy = ma.getElementAt(1, 1);
27918         final double sz = ma.getElementAt(2, 2);
27919         final double mxy = ma.getElementAt(0, 1);
27920         final double mxz = ma.getElementAt(0, 2);
27921         final double myx = ma.getElementAt(1, 0);
27922         final double myz = ma.getElementAt(1, 2);
27923         final double mzx = ma.getElementAt(2, 0);
27924         final double mzy = ma.getElementAt(2, 1);
27925 
27926         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27927         final double latitude = Math.toRadians(
27928                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27929         final double longitude = Math.toRadians(
27930                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27931         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27932         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27933         final NEDVelocity nedVelocity = new NEDVelocity();
27934         final ECEFPosition ecefPosition = new ECEFPosition();
27935         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27936         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27937                 ecefPosition, ecefVelocity);
27938 
27939         KnownPositionAccelerometerCalibrator calibrator =
27940                 new KnownPositionAccelerometerCalibrator(nedPosition,
27941                         ba, ma, this);
27942 
27943         // check default values
27944         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27945         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27946         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27947         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27948         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27949         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27950         final Acceleration bx2 = new Acceleration(0.0,
27951                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27952         calibrator.getInitialBiasXAsAcceleration(bx2);
27953         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27954         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27955         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27956         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27957         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27958         final Acceleration by2 = new Acceleration(0.0,
27959                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27960         calibrator.getInitialBiasYAsAcceleration(by2);
27961         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27962         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27963         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27964         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27965         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27966         final Acceleration bz2 = new Acceleration(0.0,
27967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27968         calibrator.getInitialBiasZAsAcceleration(bz2);
27969         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27970         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27971         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27972         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27973         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27974         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27975         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27976         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27977         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27978         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27979         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27980         final double[] bias1 = calibrator.getInitialBias();
27981         assertArrayEquals(bias1, bias, 0.0);
27982         final double[] bias2 = new double[3];
27983         calibrator.getInitialBias(bias2);
27984         assertArrayEquals(bias1, bias2, 0.0);
27985         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27986         assertEquals(b1, ba);
27987         final Matrix b2 = new Matrix(3, 1);
27988         calibrator.getInitialBiasAsMatrix(b2);
27989         assertEquals(b1, b2);
27990         final Matrix ma1 = new Matrix(3, 3);
27991         ma1.setSubmatrix(0, 0,
27992                 2, 2,
27993                 new double[]{ sx, myx, mzx,
27994                         mxy, sy, mzy,
27995                         mxz, myz, sz});
27996         assertEquals(calibrator.getInitialMa(), ma1);
27997         final Matrix ma2 = new Matrix(3, 3);
27998         calibrator.getInitialMa(ma2);
27999         assertEquals(ma1, ma2);
28000         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28001         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28002         final NEDPosition nedPosition1 = new NEDPosition();
28003         assertTrue(calibrator.getNedPosition(nedPosition1));
28004         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28005         assertNull(calibrator.getMeasurements());
28006         assertFalse(calibrator.isCommonAxisUsed());
28007         assertSame(calibrator.getListener(), this);
28008         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28009         assertFalse(calibrator.isReady());
28010         assertFalse(calibrator.isRunning());
28011         assertNull(calibrator.getEstimatedBiases());
28012         assertFalse(calibrator.getEstimatedBiases(null));
28013         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28014         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28015         assertNull(calibrator.getEstimatedBiasFx());
28016         assertNull(calibrator.getEstimatedBiasFy());
28017         assertNull(calibrator.getEstimatedBiasFz());
28018         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28019         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28020         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28021         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28022         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28023         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28024         assertNull(calibrator.getEstimatedMa());
28025         assertNull(calibrator.getEstimatedSx());
28026         assertNull(calibrator.getEstimatedSy());
28027         assertNull(calibrator.getEstimatedSz());
28028         assertNull(calibrator.getEstimatedMxy());
28029         assertNull(calibrator.getEstimatedMxz());
28030         assertNull(calibrator.getEstimatedMyx());
28031         assertNull(calibrator.getEstimatedMyz());
28032         assertNull(calibrator.getEstimatedMzx());
28033         assertNull(calibrator.getEstimatedMzy());
28034         assertNull(calibrator.getEstimatedCovariance());
28035         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28036 
28037         // Force IllegalArgumentException
28038         calibrator = null;
28039         try {
28040             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28041                     new Matrix(1, 1), ma, this);
28042             fail("IllegalArgumentException expected but not thrown");
28043         } catch (final IllegalArgumentException ignore) {
28044         }
28045         try {
28046             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28047                     new Matrix(1, 3), ma, this);
28048             fail("IllegalArgumentException expected but not thrown");
28049         } catch (final IllegalArgumentException ignore) {
28050         }
28051         try {
28052             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28053                     ba, new Matrix(1, 3), this);
28054             fail("IllegalArgumentException expected but not thrown");
28055         } catch (final IllegalArgumentException ignore) {
28056         }
28057         try {
28058             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28059                     ba, new Matrix(3, 1), this);
28060             fail("IllegalArgumentException expected but not thrown");
28061         } catch (final IllegalArgumentException ignore) {
28062         }
28063         assertNull(calibrator);
28064     }
28065 
28066     @Test
28067     public void testConstructor229() throws WrongSizeException {
28068         final Collection<StandardDeviationBodyKinematics> measurements =
28069                 Collections.emptyList();
28070 
28071         final Matrix ba = generateBa();
28072         final double[] bias = ba.getBuffer();
28073         final double biasX = ba.getElementAtIndex(0);
28074         final double biasY = ba.getElementAtIndex(1);
28075         final double biasZ = ba.getElementAtIndex(2);
28076 
28077         final Matrix ma = generateMaCommonAxis();
28078         final double sx = ma.getElementAt(0, 0);
28079         final double sy = ma.getElementAt(1, 1);
28080         final double sz = ma.getElementAt(2, 2);
28081         final double mxy = ma.getElementAt(0, 1);
28082         final double mxz = ma.getElementAt(0, 2);
28083         final double myx = ma.getElementAt(1, 0);
28084         final double myz = ma.getElementAt(1, 2);
28085         final double mzx = ma.getElementAt(2, 0);
28086         final double mzy = ma.getElementAt(2, 1);
28087 
28088         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28089         final double latitude = Math.toRadians(
28090                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28091         final double longitude = Math.toRadians(
28092                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28093         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28094         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28095         final NEDVelocity nedVelocity = new NEDVelocity();
28096         final ECEFPosition ecefPosition = new ECEFPosition();
28097         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28098         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28099                 ecefPosition, ecefVelocity);
28100 
28101         KnownPositionAccelerometerCalibrator calibrator =
28102                 new KnownPositionAccelerometerCalibrator(nedPosition,
28103                         measurements, ba, ma);
28104 
28105         // check default values
28106         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28107         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28108         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28109         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28110         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28111         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28112         final Acceleration bx2 = new Acceleration(0.0,
28113                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28114         calibrator.getInitialBiasXAsAcceleration(bx2);
28115         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28116         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28117         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28118         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28119         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28120         final Acceleration by2 = new Acceleration(0.0,
28121                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28122         calibrator.getInitialBiasYAsAcceleration(by2);
28123         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28124         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28125         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28126         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28127         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28128         final Acceleration bz2 = new Acceleration(0.0,
28129                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28130         calibrator.getInitialBiasZAsAcceleration(bz2);
28131         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28132         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28133         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28134         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28135         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28136         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28137         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28138         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28139         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28140         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28141         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28142         final double[] bias1 = calibrator.getInitialBias();
28143         assertArrayEquals(bias1, bias, 0.0);
28144         final double[] bias2 = new double[3];
28145         calibrator.getInitialBias(bias2);
28146         assertArrayEquals(bias1, bias2, 0.0);
28147         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28148         assertEquals(b1, ba);
28149         final Matrix b2 = new Matrix(3, 1);
28150         calibrator.getInitialBiasAsMatrix(b2);
28151         assertEquals(b1, b2);
28152         final Matrix ma1 = new Matrix(3, 3);
28153         ma1.setSubmatrix(0, 0,
28154                 2, 2,
28155                 new double[]{ sx, myx, mzx,
28156                         mxy, sy, mzy,
28157                         mxz, myz, sz});
28158         assertEquals(calibrator.getInitialMa(), ma1);
28159         final Matrix ma2 = new Matrix(3, 3);
28160         calibrator.getInitialMa(ma2);
28161         assertEquals(ma1, ma2);
28162         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28163         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28164         final NEDPosition nedPosition1 = new NEDPosition();
28165         assertTrue(calibrator.getNedPosition(nedPosition1));
28166         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28167         assertSame(calibrator.getMeasurements(), measurements);
28168         assertFalse(calibrator.isCommonAxisUsed());
28169         assertNull(calibrator.getListener());
28170         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28171         assertFalse(calibrator.isReady());
28172         assertFalse(calibrator.isRunning());
28173         assertNull(calibrator.getEstimatedBiases());
28174         assertFalse(calibrator.getEstimatedBiases(null));
28175         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28176         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28177         assertNull(calibrator.getEstimatedBiasFx());
28178         assertNull(calibrator.getEstimatedBiasFy());
28179         assertNull(calibrator.getEstimatedBiasFz());
28180         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28181         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28182         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28183         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28184         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28185         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28186         assertNull(calibrator.getEstimatedMa());
28187         assertNull(calibrator.getEstimatedSx());
28188         assertNull(calibrator.getEstimatedSy());
28189         assertNull(calibrator.getEstimatedSz());
28190         assertNull(calibrator.getEstimatedMxy());
28191         assertNull(calibrator.getEstimatedMxz());
28192         assertNull(calibrator.getEstimatedMyx());
28193         assertNull(calibrator.getEstimatedMyz());
28194         assertNull(calibrator.getEstimatedMzx());
28195         assertNull(calibrator.getEstimatedMzy());
28196         assertNull(calibrator.getEstimatedCovariance());
28197         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28198 
28199         // Force IllegalArgumentException
28200         calibrator = null;
28201         try {
28202             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28203                     measurements, new Matrix(1, 1), ma);
28204             fail("IllegalArgumentException expected but not thrown");
28205         } catch (final IllegalArgumentException ignore) {
28206         }
28207         try {
28208             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28209                     measurements, new Matrix(1, 3), ma);
28210             fail("IllegalArgumentException expected but not thrown");
28211         } catch (final IllegalArgumentException ignore) {
28212         }
28213         try {
28214             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28215                     measurements, ba, new Matrix(1, 3));
28216             fail("IllegalArgumentException expected but not thrown");
28217         } catch (final IllegalArgumentException ignore) {
28218         }
28219         try {
28220             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28221                     measurements, ba, new Matrix(3, 1));
28222             fail("IllegalArgumentException expected but not thrown");
28223         } catch (final IllegalArgumentException ignore) {
28224         }
28225         assertNull(calibrator);
28226     }
28227 
28228     @Test
28229     public void testConstructor230() throws WrongSizeException {
28230         final Collection<StandardDeviationBodyKinematics> measurements =
28231                 Collections.emptyList();
28232 
28233         final Matrix ba = generateBa();
28234         final double[] bias = ba.getBuffer();
28235         final double biasX = ba.getElementAtIndex(0);
28236         final double biasY = ba.getElementAtIndex(1);
28237         final double biasZ = ba.getElementAtIndex(2);
28238 
28239         final Matrix ma = generateMaCommonAxis();
28240         final double sx = ma.getElementAt(0, 0);
28241         final double sy = ma.getElementAt(1, 1);
28242         final double sz = ma.getElementAt(2, 2);
28243         final double mxy = ma.getElementAt(0, 1);
28244         final double mxz = ma.getElementAt(0, 2);
28245         final double myx = ma.getElementAt(1, 0);
28246         final double myz = ma.getElementAt(1, 2);
28247         final double mzx = ma.getElementAt(2, 0);
28248         final double mzy = ma.getElementAt(2, 1);
28249 
28250         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28251         final double latitude = Math.toRadians(
28252                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28253         final double longitude = Math.toRadians(
28254                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28255         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28256         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28257         final NEDVelocity nedVelocity = new NEDVelocity();
28258         final ECEFPosition ecefPosition = new ECEFPosition();
28259         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28260         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28261                 ecefPosition, ecefVelocity);
28262 
28263         KnownPositionAccelerometerCalibrator calibrator =
28264                 new KnownPositionAccelerometerCalibrator(nedPosition,
28265                         measurements, ba, ma, this);
28266 
28267         // check default values
28268         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28269         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28270         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28271         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28272         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28273         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28274         final Acceleration bx2 = new Acceleration(0.0,
28275                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28276         calibrator.getInitialBiasXAsAcceleration(bx2);
28277         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28278         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28279         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28280         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28281         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282         final Acceleration by2 = new Acceleration(0.0,
28283                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28284         calibrator.getInitialBiasYAsAcceleration(by2);
28285         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28286         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28287         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28288         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28289         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28290         final Acceleration bz2 = new Acceleration(0.0,
28291                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28292         calibrator.getInitialBiasZAsAcceleration(bz2);
28293         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28294         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28295         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28296         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28297         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28298         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28299         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28300         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28301         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28302         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28303         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28304         final double[] bias1 = calibrator.getInitialBias();
28305         assertArrayEquals(bias1, bias, 0.0);
28306         final double[] bias2 = new double[3];
28307         calibrator.getInitialBias(bias2);
28308         assertArrayEquals(bias1, bias2, 0.0);
28309         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28310         assertEquals(b1, ba);
28311         final Matrix b2 = new Matrix(3, 1);
28312         calibrator.getInitialBiasAsMatrix(b2);
28313         assertEquals(b1, b2);
28314         final Matrix ma1 = new Matrix(3, 3);
28315         ma1.setSubmatrix(0, 0,
28316                 2, 2,
28317                 new double[]{ sx, myx, mzx,
28318                         mxy, sy, mzy,
28319                         mxz, myz, sz});
28320         assertEquals(calibrator.getInitialMa(), ma1);
28321         final Matrix ma2 = new Matrix(3, 3);
28322         calibrator.getInitialMa(ma2);
28323         assertEquals(ma1, ma2);
28324         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28325         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28326         final NEDPosition nedPosition1 = new NEDPosition();
28327         assertTrue(calibrator.getNedPosition(nedPosition1));
28328         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28329         assertSame(calibrator.getMeasurements(), measurements);
28330         assertFalse(calibrator.isCommonAxisUsed());
28331         assertSame(calibrator.getListener(), this);
28332         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28333         assertFalse(calibrator.isReady());
28334         assertFalse(calibrator.isRunning());
28335         assertNull(calibrator.getEstimatedBiases());
28336         assertFalse(calibrator.getEstimatedBiases(null));
28337         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28338         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28339         assertNull(calibrator.getEstimatedBiasFx());
28340         assertNull(calibrator.getEstimatedBiasFy());
28341         assertNull(calibrator.getEstimatedBiasFz());
28342         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28343         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28344         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28345         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28346         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28347         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28348         assertNull(calibrator.getEstimatedMa());
28349         assertNull(calibrator.getEstimatedSx());
28350         assertNull(calibrator.getEstimatedSy());
28351         assertNull(calibrator.getEstimatedSz());
28352         assertNull(calibrator.getEstimatedMxy());
28353         assertNull(calibrator.getEstimatedMxz());
28354         assertNull(calibrator.getEstimatedMyx());
28355         assertNull(calibrator.getEstimatedMyz());
28356         assertNull(calibrator.getEstimatedMzx());
28357         assertNull(calibrator.getEstimatedMzy());
28358         assertNull(calibrator.getEstimatedCovariance());
28359         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28360 
28361         // Force IllegalArgumentException
28362         calibrator = null;
28363         try {
28364             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28365                     measurements, new Matrix(1, 1), ma, this);
28366             fail("IllegalArgumentException expected but not thrown");
28367         } catch (final IllegalArgumentException ignore) {
28368         }
28369         try {
28370             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28371                     measurements, new Matrix(1, 3), ma, this);
28372             fail("IllegalArgumentException expected but not thrown");
28373         } catch (final IllegalArgumentException ignore) {
28374         }
28375         try {
28376             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28377                     measurements, ba, new Matrix(1, 3), this);
28378             fail("IllegalArgumentException expected but not thrown");
28379         } catch (final IllegalArgumentException ignore) {
28380         }
28381         try {
28382             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28383                     measurements, ba, new Matrix(3, 1), this);
28384             fail("IllegalArgumentException expected but not thrown");
28385         } catch (final IllegalArgumentException ignore) {
28386         }
28387         assertNull(calibrator);
28388     }
28389 
28390     @Test
28391     public void testConstructor231() throws WrongSizeException {
28392         final Matrix ba = generateBa();
28393         final double[] bias = ba.getBuffer();
28394         final double biasX = ba.getElementAtIndex(0);
28395         final double biasY = ba.getElementAtIndex(1);
28396         final double biasZ = ba.getElementAtIndex(2);
28397 
28398         final Matrix ma = generateMaCommonAxis();
28399         final double sx = ma.getElementAt(0, 0);
28400         final double sy = ma.getElementAt(1, 1);
28401         final double sz = ma.getElementAt(2, 2);
28402         final double mxy = ma.getElementAt(0, 1);
28403         final double mxz = ma.getElementAt(0, 2);
28404         final double myx = ma.getElementAt(1, 0);
28405         final double myz = ma.getElementAt(1, 2);
28406         final double mzx = ma.getElementAt(2, 0);
28407         final double mzy = ma.getElementAt(2, 1);
28408 
28409         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28410         final double latitude = Math.toRadians(
28411                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28412         final double longitude = Math.toRadians(
28413                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28414         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28415         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28416         final NEDVelocity nedVelocity = new NEDVelocity();
28417         final ECEFPosition ecefPosition = new ECEFPosition();
28418         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28419         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28420                 ecefPosition, ecefVelocity);
28421 
28422         KnownPositionAccelerometerCalibrator calibrator =
28423                 new KnownPositionAccelerometerCalibrator(nedPosition,
28424                         true, ba, ma);
28425 
28426         // check default values
28427         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28428         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28429         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28430         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28431         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28432         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28433         final Acceleration bx2 = new Acceleration(0.0,
28434                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28435         calibrator.getInitialBiasXAsAcceleration(bx2);
28436         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28437         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28438         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28439         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28440         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28441         final Acceleration by2 = new Acceleration(0.0,
28442                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28443         calibrator.getInitialBiasYAsAcceleration(by2);
28444         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28445         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28446         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28447         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28448         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28449         final Acceleration bz2 = new Acceleration(0.0,
28450                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28451         calibrator.getInitialBiasZAsAcceleration(bz2);
28452         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28453         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28454         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28455         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28456         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28457         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28458         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28459         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28460         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28461         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28462         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28463         final double[] bias1 = calibrator.getInitialBias();
28464         assertArrayEquals(bias1, bias, 0.0);
28465         final double[] bias2 = new double[3];
28466         calibrator.getInitialBias(bias2);
28467         assertArrayEquals(bias1, bias2, 0.0);
28468         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28469         assertEquals(b1, ba);
28470         final Matrix b2 = new Matrix(3, 1);
28471         calibrator.getInitialBiasAsMatrix(b2);
28472         assertEquals(b1, b2);
28473         final Matrix ma1 = new Matrix(3, 3);
28474         ma1.setSubmatrix(0, 0,
28475                 2, 2,
28476                 new double[]{ sx, myx, mzx,
28477                         mxy, sy, mzy,
28478                         mxz, myz, sz});
28479         assertEquals(calibrator.getInitialMa(), ma1);
28480         final Matrix ma2 = new Matrix(3, 3);
28481         calibrator.getInitialMa(ma2);
28482         assertEquals(ma1, ma2);
28483         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28484         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28485         final NEDPosition nedPosition1 = new NEDPosition();
28486         assertTrue(calibrator.getNedPosition(nedPosition1));
28487         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28488         assertNull(calibrator.getMeasurements());
28489         assertTrue(calibrator.isCommonAxisUsed());
28490         assertNull(calibrator.getListener());
28491         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28492         assertFalse(calibrator.isReady());
28493         assertFalse(calibrator.isRunning());
28494         assertNull(calibrator.getEstimatedBiases());
28495         assertFalse(calibrator.getEstimatedBiases(null));
28496         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28497         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28498         assertNull(calibrator.getEstimatedBiasFx());
28499         assertNull(calibrator.getEstimatedBiasFy());
28500         assertNull(calibrator.getEstimatedBiasFz());
28501         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28502         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28503         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28504         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28505         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28506         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28507         assertNull(calibrator.getEstimatedMa());
28508         assertNull(calibrator.getEstimatedSx());
28509         assertNull(calibrator.getEstimatedSy());
28510         assertNull(calibrator.getEstimatedSz());
28511         assertNull(calibrator.getEstimatedMxy());
28512         assertNull(calibrator.getEstimatedMxz());
28513         assertNull(calibrator.getEstimatedMyx());
28514         assertNull(calibrator.getEstimatedMyz());
28515         assertNull(calibrator.getEstimatedMzx());
28516         assertNull(calibrator.getEstimatedMzy());
28517         assertNull(calibrator.getEstimatedCovariance());
28518         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28519 
28520         // Force IllegalArgumentException
28521         calibrator = null;
28522         try {
28523             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28524                     true, new Matrix(1, 1), ma);
28525             fail("IllegalArgumentException expected but not thrown");
28526         } catch (final IllegalArgumentException ignore) {
28527         }
28528         try {
28529             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28530                     true, new Matrix(1, 3), ma);
28531             fail("IllegalArgumentException expected but not thrown");
28532         } catch (final IllegalArgumentException ignore) {
28533         }
28534         try {
28535             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28536                     true, ba, new Matrix(1, 3));
28537             fail("IllegalArgumentException expected but not thrown");
28538         } catch (final IllegalArgumentException ignore) {
28539         }
28540         try {
28541             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28542                     true, ba, new Matrix(3, 1));
28543             fail("IllegalArgumentException expected but not thrown");
28544         } catch (final IllegalArgumentException ignore) {
28545         }
28546         assertNull(calibrator);
28547     }
28548 
28549     @Test
28550     public void testConstructor232() throws WrongSizeException {
28551         final Matrix ba = generateBa();
28552         final double[] bias = ba.getBuffer();
28553         final double biasX = ba.getElementAtIndex(0);
28554         final double biasY = ba.getElementAtIndex(1);
28555         final double biasZ = ba.getElementAtIndex(2);
28556 
28557         final Matrix ma = generateMaCommonAxis();
28558         final double sx = ma.getElementAt(0, 0);
28559         final double sy = ma.getElementAt(1, 1);
28560         final double sz = ma.getElementAt(2, 2);
28561         final double mxy = ma.getElementAt(0, 1);
28562         final double mxz = ma.getElementAt(0, 2);
28563         final double myx = ma.getElementAt(1, 0);
28564         final double myz = ma.getElementAt(1, 2);
28565         final double mzx = ma.getElementAt(2, 0);
28566         final double mzy = ma.getElementAt(2, 1);
28567 
28568         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28569         final double latitude = Math.toRadians(
28570                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28571         final double longitude = Math.toRadians(
28572                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28573         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28574         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28575         final NEDVelocity nedVelocity = new NEDVelocity();
28576         final ECEFPosition ecefPosition = new ECEFPosition();
28577         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28578         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28579                 ecefPosition, ecefVelocity);
28580 
28581         KnownPositionAccelerometerCalibrator calibrator =
28582                 new KnownPositionAccelerometerCalibrator(nedPosition,
28583                         true, ba, ma, this);
28584 
28585         // check default values
28586         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28587         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28588         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28589         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28590         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28591         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28592         final Acceleration bx2 = new Acceleration(0.0,
28593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28594         calibrator.getInitialBiasXAsAcceleration(bx2);
28595         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28596         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28597         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28598         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28599         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28600         final Acceleration by2 = new Acceleration(0.0,
28601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28602         calibrator.getInitialBiasYAsAcceleration(by2);
28603         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28604         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28605         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28606         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28607         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28608         final Acceleration bz2 = new Acceleration(0.0,
28609                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28610         calibrator.getInitialBiasZAsAcceleration(bz2);
28611         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28612         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28613         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28614         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28615         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28616         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28617         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28618         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28619         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28620         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28621         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28622         final double[] bias1 = calibrator.getInitialBias();
28623         assertArrayEquals(bias1, bias, 0.0);
28624         final double[] bias2 = new double[3];
28625         calibrator.getInitialBias(bias2);
28626         assertArrayEquals(bias1, bias2, 0.0);
28627         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28628         assertEquals(b1, ba);
28629         final Matrix b2 = new Matrix(3, 1);
28630         calibrator.getInitialBiasAsMatrix(b2);
28631         assertEquals(b1, b2);
28632         final Matrix ma1 = new Matrix(3, 3);
28633         ma1.setSubmatrix(0, 0,
28634                 2, 2,
28635                 new double[]{ sx, myx, mzx,
28636                         mxy, sy, mzy,
28637                         mxz, myz, sz});
28638         assertEquals(calibrator.getInitialMa(), ma1);
28639         final Matrix ma2 = new Matrix(3, 3);
28640         calibrator.getInitialMa(ma2);
28641         assertEquals(ma1, ma2);
28642         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28643         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28644         final NEDPosition nedPosition1 = new NEDPosition();
28645         assertTrue(calibrator.getNedPosition(nedPosition1));
28646         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28647         assertNull(calibrator.getMeasurements());
28648         assertTrue(calibrator.isCommonAxisUsed());
28649         assertSame(calibrator.getListener(), this);
28650         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28651         assertFalse(calibrator.isReady());
28652         assertFalse(calibrator.isRunning());
28653         assertNull(calibrator.getEstimatedBiases());
28654         assertFalse(calibrator.getEstimatedBiases(null));
28655         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28656         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28657         assertNull(calibrator.getEstimatedBiasFx());
28658         assertNull(calibrator.getEstimatedBiasFy());
28659         assertNull(calibrator.getEstimatedBiasFz());
28660         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28661         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28662         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28663         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28664         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28665         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28666         assertNull(calibrator.getEstimatedMa());
28667         assertNull(calibrator.getEstimatedSx());
28668         assertNull(calibrator.getEstimatedSy());
28669         assertNull(calibrator.getEstimatedSz());
28670         assertNull(calibrator.getEstimatedMxy());
28671         assertNull(calibrator.getEstimatedMxz());
28672         assertNull(calibrator.getEstimatedMyx());
28673         assertNull(calibrator.getEstimatedMyz());
28674         assertNull(calibrator.getEstimatedMzx());
28675         assertNull(calibrator.getEstimatedMzy());
28676         assertNull(calibrator.getEstimatedCovariance());
28677         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28678 
28679         // Force IllegalArgumentException
28680         calibrator = null;
28681         try {
28682             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28683                     true, new Matrix(1, 1), ma, this);
28684             fail("IllegalArgumentException expected but not thrown");
28685         } catch (final IllegalArgumentException ignore) {
28686         }
28687         try {
28688             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28689                     true, new Matrix(1, 3), ma, this);
28690             fail("IllegalArgumentException expected but not thrown");
28691         } catch (final IllegalArgumentException ignore) {
28692         }
28693         try {
28694             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28695                     true, ba, new Matrix(1, 3), this);
28696             fail("IllegalArgumentException expected but not thrown");
28697         } catch (final IllegalArgumentException ignore) {
28698         }
28699         try {
28700             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28701                     true, ba, new Matrix(3, 1), this);
28702             fail("IllegalArgumentException expected but not thrown");
28703         } catch (final IllegalArgumentException ignore) {
28704         }
28705         assertNull(calibrator);
28706     }
28707 
28708     @Test
28709     public void testConstructor233() throws WrongSizeException {
28710         final Collection<StandardDeviationBodyKinematics> measurements =
28711                 Collections.emptyList();
28712 
28713         final Matrix ba = generateBa();
28714         final double[] bias = ba.getBuffer();
28715         final double biasX = ba.getElementAtIndex(0);
28716         final double biasY = ba.getElementAtIndex(1);
28717         final double biasZ = ba.getElementAtIndex(2);
28718 
28719         final Matrix ma = generateMaCommonAxis();
28720         final double sx = ma.getElementAt(0, 0);
28721         final double sy = ma.getElementAt(1, 1);
28722         final double sz = ma.getElementAt(2, 2);
28723         final double mxy = ma.getElementAt(0, 1);
28724         final double mxz = ma.getElementAt(0, 2);
28725         final double myx = ma.getElementAt(1, 0);
28726         final double myz = ma.getElementAt(1, 2);
28727         final double mzx = ma.getElementAt(2, 0);
28728         final double mzy = ma.getElementAt(2, 1);
28729 
28730         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28731         final double latitude = Math.toRadians(
28732                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28733         final double longitude = Math.toRadians(
28734                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28735         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28736         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28737         final NEDVelocity nedVelocity = new NEDVelocity();
28738         final ECEFPosition ecefPosition = new ECEFPosition();
28739         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28740         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28741                 ecefPosition, ecefVelocity);
28742 
28743         KnownPositionAccelerometerCalibrator calibrator =
28744                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28745                         true, ba, ma);
28746 
28747         // check default values
28748         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28749         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28750         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28751         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28752         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28753         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28754         final Acceleration bx2 = new Acceleration(0.0,
28755                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28756         calibrator.getInitialBiasXAsAcceleration(bx2);
28757         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28758         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28759         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28760         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28761         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28762         final Acceleration by2 = new Acceleration(0.0,
28763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28764         calibrator.getInitialBiasYAsAcceleration(by2);
28765         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28766         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28767         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28768         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28769         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28770         final Acceleration bz2 = new Acceleration(0.0,
28771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28772         calibrator.getInitialBiasZAsAcceleration(bz2);
28773         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28774         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28775         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28776         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28777         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28778         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28779         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28780         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28781         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28782         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28783         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28784         final double[] bias1 = calibrator.getInitialBias();
28785         assertArrayEquals(bias1, bias, 0.0);
28786         final double[] bias2 = new double[3];
28787         calibrator.getInitialBias(bias2);
28788         assertArrayEquals(bias1, bias2, 0.0);
28789         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28790         assertEquals(b1, ba);
28791         final Matrix b2 = new Matrix(3, 1);
28792         calibrator.getInitialBiasAsMatrix(b2);
28793         assertEquals(b1, b2);
28794         final Matrix ma1 = new Matrix(3, 3);
28795         ma1.setSubmatrix(0, 0,
28796                 2, 2,
28797                 new double[]{ sx, myx, mzx,
28798                         mxy, sy, mzy,
28799                         mxz, myz, sz});
28800         assertEquals(calibrator.getInitialMa(), ma1);
28801         final Matrix ma2 = new Matrix(3, 3);
28802         calibrator.getInitialMa(ma2);
28803         assertEquals(ma1, ma2);
28804         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28805         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28806         final NEDPosition nedPosition1 = new NEDPosition();
28807         assertTrue(calibrator.getNedPosition(nedPosition1));
28808         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28809         assertSame(calibrator.getMeasurements(), measurements);
28810         assertTrue(calibrator.isCommonAxisUsed());
28811         assertNull(calibrator.getListener());
28812         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28813         assertFalse(calibrator.isReady());
28814         assertFalse(calibrator.isRunning());
28815         assertNull(calibrator.getEstimatedBiases());
28816         assertFalse(calibrator.getEstimatedBiases(null));
28817         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28818         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28819         assertNull(calibrator.getEstimatedBiasFx());
28820         assertNull(calibrator.getEstimatedBiasFy());
28821         assertNull(calibrator.getEstimatedBiasFz());
28822         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28823         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28824         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28825         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28826         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28827         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28828         assertNull(calibrator.getEstimatedMa());
28829         assertNull(calibrator.getEstimatedSx());
28830         assertNull(calibrator.getEstimatedSy());
28831         assertNull(calibrator.getEstimatedSz());
28832         assertNull(calibrator.getEstimatedMxy());
28833         assertNull(calibrator.getEstimatedMxz());
28834         assertNull(calibrator.getEstimatedMyx());
28835         assertNull(calibrator.getEstimatedMyz());
28836         assertNull(calibrator.getEstimatedMzx());
28837         assertNull(calibrator.getEstimatedMzy());
28838         assertNull(calibrator.getEstimatedCovariance());
28839         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28840 
28841         // Force IllegalArgumentException
28842         calibrator = null;
28843         try {
28844             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28845                     measurements, true,
28846                     new Matrix(1, 1), ma);
28847             fail("IllegalArgumentException expected but not thrown");
28848         } catch (final IllegalArgumentException ignore) {
28849         }
28850         try {
28851             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28852                     measurements, true,
28853                     new Matrix(1, 3), ma);
28854             fail("IllegalArgumentException expected but not thrown");
28855         } catch (final IllegalArgumentException ignore) {
28856         }
28857         try {
28858             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28859                     measurements, true,
28860                     ba, new Matrix(1, 3));
28861             fail("IllegalArgumentException expected but not thrown");
28862         } catch (final IllegalArgumentException ignore) {
28863         }
28864         try {
28865             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28866                     measurements, true,
28867                     ba, new Matrix(3, 1));
28868             fail("IllegalArgumentException expected but not thrown");
28869         } catch (final IllegalArgumentException ignore) {
28870         }
28871         assertNull(calibrator);
28872     }
28873 
28874     @Test
28875     public void testConstructor234() throws WrongSizeException {
28876         final Collection<StandardDeviationBodyKinematics> measurements =
28877                 Collections.emptyList();
28878 
28879         final Matrix ba = generateBa();
28880         final double[] bias = ba.getBuffer();
28881         final double biasX = ba.getElementAtIndex(0);
28882         final double biasY = ba.getElementAtIndex(1);
28883         final double biasZ = ba.getElementAtIndex(2);
28884 
28885         final Matrix ma = generateMaCommonAxis();
28886         final double sx = ma.getElementAt(0, 0);
28887         final double sy = ma.getElementAt(1, 1);
28888         final double sz = ma.getElementAt(2, 2);
28889         final double mxy = ma.getElementAt(0, 1);
28890         final double mxz = ma.getElementAt(0, 2);
28891         final double myx = ma.getElementAt(1, 0);
28892         final double myz = ma.getElementAt(1, 2);
28893         final double mzx = ma.getElementAt(2, 0);
28894         final double mzy = ma.getElementAt(2, 1);
28895 
28896         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28897         final double latitude = Math.toRadians(
28898                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28899         final double longitude = Math.toRadians(
28900                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28901         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28902         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28903         final NEDVelocity nedVelocity = new NEDVelocity();
28904         final ECEFPosition ecefPosition = new ECEFPosition();
28905         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28906         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28907                 ecefPosition, ecefVelocity);
28908 
28909         KnownPositionAccelerometerCalibrator calibrator =
28910                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28911                         true, ba, ma, this);
28912 
28913         // check default values
28914         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28915         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28916         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28917         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28918         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28919         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28920         final Acceleration bx2 = new Acceleration(0.0,
28921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28922         calibrator.getInitialBiasXAsAcceleration(bx2);
28923         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28924         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28925         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28926         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28927         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28928         final Acceleration by2 = new Acceleration(0.0,
28929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28930         calibrator.getInitialBiasYAsAcceleration(by2);
28931         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28932         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28933         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28934         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28935         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28936         final Acceleration bz2 = new Acceleration(0.0,
28937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28938         calibrator.getInitialBiasZAsAcceleration(bz2);
28939         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28940         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28941         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28942         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28943         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28944         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28945         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28946         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28947         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28948         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28949         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28950         final double[] bias1 = calibrator.getInitialBias();
28951         assertArrayEquals(bias1, bias, 0.0);
28952         final double[] bias2 = new double[3];
28953         calibrator.getInitialBias(bias2);
28954         assertArrayEquals(bias1, bias2, 0.0);
28955         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28956         assertEquals(b1, ba);
28957         final Matrix b2 = new Matrix(3, 1);
28958         calibrator.getInitialBiasAsMatrix(b2);
28959         assertEquals(b1, b2);
28960         final Matrix ma1 = new Matrix(3, 3);
28961         ma1.setSubmatrix(0, 0,
28962                 2, 2,
28963                 new double[]{ sx, myx, mzx,
28964                         mxy, sy, mzy,
28965                         mxz, myz, sz});
28966         assertEquals(calibrator.getInitialMa(), ma1);
28967         final Matrix ma2 = new Matrix(3, 3);
28968         calibrator.getInitialMa(ma2);
28969         assertEquals(ma1, ma2);
28970         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28971         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28972         final NEDPosition nedPosition1 = new NEDPosition();
28973         assertTrue(calibrator.getNedPosition(nedPosition1));
28974         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28975         assertSame(calibrator.getMeasurements(), measurements);
28976         assertTrue(calibrator.isCommonAxisUsed());
28977         assertSame(calibrator.getListener(), this);
28978         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28979         assertFalse(calibrator.isReady());
28980         assertFalse(calibrator.isRunning());
28981         assertNull(calibrator.getEstimatedBiases());
28982         assertFalse(calibrator.getEstimatedBiases(null));
28983         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28984         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28985         assertNull(calibrator.getEstimatedBiasFx());
28986         assertNull(calibrator.getEstimatedBiasFy());
28987         assertNull(calibrator.getEstimatedBiasFz());
28988         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28989         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28990         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28991         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28992         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28993         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28994         assertNull(calibrator.getEstimatedMa());
28995         assertNull(calibrator.getEstimatedSx());
28996         assertNull(calibrator.getEstimatedSy());
28997         assertNull(calibrator.getEstimatedSz());
28998         assertNull(calibrator.getEstimatedMxy());
28999         assertNull(calibrator.getEstimatedMxz());
29000         assertNull(calibrator.getEstimatedMyx());
29001         assertNull(calibrator.getEstimatedMyz());
29002         assertNull(calibrator.getEstimatedMzx());
29003         assertNull(calibrator.getEstimatedMzy());
29004         assertNull(calibrator.getEstimatedCovariance());
29005         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29006 
29007         // Force IllegalArgumentException
29008         calibrator = null;
29009         try {
29010             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29011                     measurements, true,
29012                     new Matrix(1, 1), ma, this);
29013             fail("IllegalArgumentException expected but not thrown");
29014         } catch (final IllegalArgumentException ignore) {
29015         }
29016         try {
29017             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29018                     measurements, true,
29019                     new Matrix(1, 3), ma, this);
29020             fail("IllegalArgumentException expected but not thrown");
29021         } catch (final IllegalArgumentException ignore) {
29022         }
29023         try {
29024             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29025                     measurements, true,
29026                     ba, new Matrix(1, 3), this);
29027             fail("IllegalArgumentException expected but not thrown");
29028         } catch (final IllegalArgumentException ignore) {
29029         }
29030         try {
29031             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29032                     measurements, true,
29033                     ba, new Matrix(3, 1), this);
29034             fail("IllegalArgumentException expected but not thrown");
29035         } catch (final IllegalArgumentException ignore) {
29036         }
29037         assertNull(calibrator);
29038     }
29039 
29040     @Test
29041     public void testGetSetInitialBiasX() throws LockedException {
29042         final KnownPositionAccelerometerCalibrator calibrator =
29043                 new KnownPositionAccelerometerCalibrator();
29044 
29045         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29046 
29047         // set new value
29048         final Matrix ba = generateBa();
29049         final double biasX = ba.getElementAtIndex(0);
29050 
29051         calibrator.setInitialBiasX(biasX);
29052 
29053         // check
29054         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29055     }
29056 
29057     @Test
29058     public void testGetSetInitialBiasY() throws LockedException {
29059         final KnownPositionAccelerometerCalibrator calibrator =
29060                 new KnownPositionAccelerometerCalibrator();
29061 
29062         assertEquals(calibrator.getInitialBiasY(),0.0, 0.0);
29063 
29064         // set new value
29065         final Matrix ba = generateBa();
29066         final double biasY = ba.getElementAtIndex(1);
29067 
29068         calibrator.setInitialBiasY(biasY);
29069 
29070         // check
29071         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29072     }
29073 
29074     @Test
29075     public void testGetSetInitialBiasZ() throws LockedException {
29076         final KnownPositionAccelerometerCalibrator calibrator =
29077                 new KnownPositionAccelerometerCalibrator();
29078 
29079         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29080 
29081         // set new value
29082         final Matrix ba = generateBa();
29083         final double biasZ = ba.getElementAtIndex(2);
29084 
29085         calibrator.setInitialBiasZ(biasZ);
29086 
29087         // check
29088         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29089     }
29090 
29091     @Test
29092     public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
29093         final KnownPositionAccelerometerCalibrator calibrator =
29094                 new KnownPositionAccelerometerCalibrator();
29095 
29096         // check default value
29097         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29098 
29099         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
29100         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29101 
29102         // set new value
29103         final Matrix ba = generateBa();
29104         final double biasX = ba.getElementAtIndex(0);
29105 
29106         final Acceleration bx2 = new Acceleration(biasX,
29107                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29108         calibrator.setInitialBiasX(bx2);
29109 
29110         // check
29111         final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
29112         final Acceleration bx4 = new Acceleration(biasX,
29113                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29114         calibrator.getInitialBiasXAsAcceleration(bx4);
29115 
29116         assertEquals(bx3, bx4);
29117     }
29118 
29119     @Test
29120     public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
29121         final KnownPositionAccelerometerCalibrator calibrator =
29122                 new KnownPositionAccelerometerCalibrator();
29123 
29124         // check default value
29125         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29126 
29127         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
29128         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29129 
29130         // set new value
29131         final Matrix ba = generateBa();
29132         final double biasY = ba.getElementAtIndex(1);
29133 
29134         final Acceleration by2 = new Acceleration(biasY,
29135                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29136         calibrator.setInitialBiasY(by2);
29137 
29138         // check
29139         final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
29140         final Acceleration by4 = new Acceleration(biasY,
29141                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29142         calibrator.getInitialBiasYAsAcceleration(by4);
29143 
29144         assertEquals(by3, by4);
29145     }
29146 
29147     @Test
29148     public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
29149         final KnownPositionAccelerometerCalibrator calibrator =
29150                 new KnownPositionAccelerometerCalibrator();
29151 
29152         // check default value
29153         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29154 
29155         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
29156         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29157 
29158         // set new value
29159         final Matrix ba = generateBa();
29160         final double biasZ = ba.getElementAtIndex(2);
29161 
29162         final Acceleration bz2 = new Acceleration(biasZ,
29163                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29164         calibrator.setInitialBiasZ(bz2);
29165 
29166         // check
29167         final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
29168         final Acceleration bz4 = new Acceleration(biasZ,
29169                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29170         calibrator.getInitialBiasZAsAcceleration(bz4);
29171 
29172         assertEquals(bz3, bz4);
29173     }
29174 
29175     @Test
29176     public void testSetInitialBias1() throws LockedException {
29177         final KnownPositionAccelerometerCalibrator calibrator =
29178                 new KnownPositionAccelerometerCalibrator();
29179 
29180         // check default values
29181         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29182         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
29183         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29184 
29185         // set new values
29186         final Matrix ba = generateBa();
29187         final double biasX = ba.getElementAtIndex(0);
29188         final double biasY = ba.getElementAtIndex(1);
29189         final double biasZ = ba.getElementAtIndex(2);
29190 
29191         calibrator.setInitialBias(biasX, biasY, biasZ);
29192 
29193         // check
29194         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29195         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29196         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29197     }
29198 
29199     @Test
29200     public void testSetInitialBias2() throws LockedException {
29201         final KnownPositionAccelerometerCalibrator calibrator =
29202                 new KnownPositionAccelerometerCalibrator();
29203 
29204         // check default values
29205         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
29206         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
29207         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
29208 
29209         // set new values
29210         final Matrix ba = generateBa();
29211         final double biasX = ba.getElementAtIndex(0);
29212         final double biasY = ba.getElementAtIndex(1);
29213         final double biasZ = ba.getElementAtIndex(2);
29214 
29215         final Acceleration bx = new Acceleration(biasX,
29216                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29217         final Acceleration by = new Acceleration(biasY,
29218                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29219         final Acceleration bz = new Acceleration(biasZ,
29220                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
29221 
29222         calibrator.setInitialBias(bx, by, bz);
29223 
29224         // check
29225         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29226         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29227         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29228     }
29229 
29230     @Test
29231     public void testGetSetInitialSx() throws WrongSizeException, LockedException {
29232         final KnownPositionAccelerometerCalibrator calibrator =
29233                 new KnownPositionAccelerometerCalibrator();
29234 
29235         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29236 
29237         // set new value
29238         final Matrix ma = generateMaGeneral();
29239         final double sx = ma.getElementAt(0, 0);
29240 
29241         calibrator.setInitialSx(sx);
29242 
29243         // check
29244         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29245     }
29246 
29247     @Test
29248     public void testGetSetInitialSy() throws WrongSizeException, LockedException {
29249         final KnownPositionAccelerometerCalibrator calibrator =
29250                 new KnownPositionAccelerometerCalibrator();
29251 
29252         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29253 
29254         // set new value
29255         final Matrix ma = generateMaGeneral();
29256         final double sy = ma.getElementAt(1, 1);
29257 
29258         calibrator.setInitialSy(sy);
29259 
29260         // check
29261         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29262     }
29263 
29264     @Test
29265     public void testGetSetInitialSz() throws WrongSizeException, LockedException {
29266         final KnownPositionAccelerometerCalibrator calibrator =
29267                 new KnownPositionAccelerometerCalibrator();
29268 
29269         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29270 
29271         // set new value
29272         final Matrix ma = generateMaGeneral();
29273         final double sz = ma.getElementAt(2, 2);
29274 
29275         calibrator.setInitialSz(sz);
29276 
29277         // check
29278         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29279     }
29280 
29281     @Test
29282     public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
29283         final KnownPositionAccelerometerCalibrator calibrator =
29284                 new KnownPositionAccelerometerCalibrator();
29285 
29286         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29287 
29288         // set new value
29289         final Matrix ma = generateMaGeneral();
29290         final double mxy = ma.getElementAt(0, 1);
29291 
29292         calibrator.setInitialMxy(mxy);
29293 
29294         // check
29295         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29296     }
29297 
29298     @Test
29299     public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
29300         final KnownPositionAccelerometerCalibrator calibrator =
29301                 new KnownPositionAccelerometerCalibrator();
29302 
29303         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29304 
29305         // set new value
29306         final Matrix ma = generateMaGeneral();
29307         final double mxz = ma.getElementAt(0, 2);
29308 
29309         calibrator.setInitialMxz(mxz);
29310 
29311         // check
29312         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29313     }
29314 
29315     @Test
29316     public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
29317         final KnownPositionAccelerometerCalibrator calibrator =
29318                 new KnownPositionAccelerometerCalibrator();
29319 
29320         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29321 
29322         // set new value
29323         final Matrix ma = generateMaGeneral();
29324         final double myx = ma.getElementAt(1, 0);
29325 
29326         calibrator.setInitialMyx(myx);
29327 
29328         // check
29329         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29330     }
29331 
29332     @Test
29333     public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
29334         final KnownPositionAccelerometerCalibrator calibrator =
29335                 new KnownPositionAccelerometerCalibrator();
29336 
29337         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29338 
29339         // set new value
29340         final Matrix ma = generateMaGeneral();
29341         final double myz = ma.getElementAt(1, 2);
29342 
29343         calibrator.setInitialMyz(myz);
29344 
29345         // check
29346         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29347     }
29348 
29349     @Test
29350     public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
29351         final KnownPositionAccelerometerCalibrator calibrator =
29352                 new KnownPositionAccelerometerCalibrator();
29353 
29354         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29355 
29356         // set new value
29357         final Matrix ma = generateMaGeneral();
29358         final double mzx = ma.getElementAt(2, 0);
29359 
29360         calibrator.setInitialMzx(mzx);
29361 
29362         // check
29363         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29364     }
29365 
29366     @Test
29367     public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
29368         final KnownPositionAccelerometerCalibrator calibrator =
29369                 new KnownPositionAccelerometerCalibrator();
29370 
29371         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29372 
29373         // set new value
29374         final Matrix ma = generateMaGeneral();
29375         final double mzy = ma.getElementAt(2, 1);
29376 
29377         calibrator.setInitialMzy(mzy);
29378 
29379         // check
29380         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29381     }
29382 
29383     @Test
29384     public void testSetInitialScalingFactors()
29385             throws WrongSizeException, LockedException {
29386 
29387         final KnownPositionAccelerometerCalibrator calibrator =
29388                 new KnownPositionAccelerometerCalibrator();
29389 
29390         // check default values
29391         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29392         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29393         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29394 
29395         // set new values
29396         final Matrix ma = generateMaGeneral();
29397         final double sx = ma.getElementAt(0, 0);
29398         final double sy = ma.getElementAt(1, 1);
29399         final double sz = ma.getElementAt(2, 2);
29400 
29401         calibrator.setInitialScalingFactors(sx, sy, sz);
29402 
29403         // check
29404         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29405         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29406         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29407     }
29408 
29409     @Test
29410     public void testSetInitialCrossCouplingErrors()
29411             throws WrongSizeException, LockedException {
29412 
29413         final KnownPositionAccelerometerCalibrator calibrator =
29414                 new KnownPositionAccelerometerCalibrator();
29415 
29416         // check default values
29417         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29418         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29419         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29420         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29421         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29422         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29423 
29424         // set new values
29425         final Matrix ma = generateMaGeneral();
29426         final double mxy = ma.getElementAt(0, 1);
29427         final double mxz = ma.getElementAt(0, 2);
29428         final double myx = ma.getElementAt(1, 0);
29429         final double myz = ma.getElementAt(1, 2);
29430         final double mzx = ma.getElementAt(2, 0);
29431         final double mzy = ma.getElementAt(2, 1);
29432 
29433         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
29434 
29435         // check
29436         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29437         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29438         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29439         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29440         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29441         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29442     }
29443 
29444     @Test
29445     public void testSetInitialScalingFactorsAndCrossCouplingErrors()
29446             throws WrongSizeException, LockedException {
29447 
29448         final KnownPositionAccelerometerCalibrator calibrator =
29449                 new KnownPositionAccelerometerCalibrator();
29450 
29451         // check default values
29452         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29453         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29454         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29455         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29456         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29457         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29458         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29459         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29460         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29461 
29462         // set new values
29463         final Matrix ma = generateMaGeneral();
29464         final double sx = ma.getElementAt(0, 0);
29465         final double sy = ma.getElementAt(1, 1);
29466         final double sz = ma.getElementAt(2, 2);
29467         final double mxy = ma.getElementAt(0, 1);
29468         final double mxz = ma.getElementAt(0, 2);
29469         final double myx = ma.getElementAt(1, 0);
29470         final double myz = ma.getElementAt(1, 2);
29471         final double mzx = ma.getElementAt(2, 0);
29472         final double mzy = ma.getElementAt(2, 1);
29473 
29474         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
29475                 mxy, mxz, myx, myz, mzx, mzy);
29476 
29477         // check
29478         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29479         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29480         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29481         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29482         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29483         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29484         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29485         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29486         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29487     }
29488 
29489     @Test
29490     public void testGetSetInitialBias() throws LockedException {
29491         final KnownPositionAccelerometerCalibrator calibrator =
29492                 new KnownPositionAccelerometerCalibrator();
29493 
29494         // check default value
29495         final double[] bias1 = calibrator.getInitialBias();
29496         assertArrayEquals(bias1, new double[3], 0.0);
29497 
29498         // set new values
29499         final Matrix ba = generateBa();
29500         final double[] bias2 = ba.getBuffer();
29501         calibrator.setInitialBias(bias2);
29502 
29503         // check
29504         final double[] bias3 = calibrator.getInitialBias();
29505         final double[] bias4 = new double[3];
29506         calibrator.getInitialBias(bias4);
29507 
29508         assertArrayEquals(bias2, bias3, 0.0);
29509         assertArrayEquals(bias2, bias4, 0.0);
29510 
29511         // Force IllegalArgumentException
29512         try {
29513             calibrator.getInitialBias(new double[1]);
29514             fail("IllegalArgumentException expected but not thrown");
29515         } catch(final IllegalArgumentException ignore) {
29516         }
29517         try {
29518             calibrator.setInitialBias(new double[1]);
29519             fail("IllegalArgumentException expected but not thrown");
29520         } catch(final IllegalArgumentException ignore) {
29521         }
29522     }
29523 
29524     @Test
29525     public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
29526             LockedException {
29527 
29528         final KnownPositionAccelerometerCalibrator calibrator =
29529                 new KnownPositionAccelerometerCalibrator();
29530 
29531         // check default value
29532         final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
29533         assertEquals(bias1, new Matrix(3, 1));
29534 
29535         // set new value
29536         final Matrix bias2 = generateBa();
29537         calibrator.setInitialBias(bias2);
29538 
29539         // check
29540         final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
29541         final Matrix bias4 = new Matrix(3, 1);
29542         calibrator.getInitialBiasAsMatrix(bias4);
29543 
29544         assertEquals(bias2, bias3);
29545         assertEquals(bias2, bias4);
29546 
29547         // Force IllegalArgumentException
29548         try {
29549             calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
29550             fail("IllegalArgumentException expected but not thrown");
29551         } catch (final IllegalArgumentException ignore) {
29552         }
29553         try {
29554             calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
29555             fail("IllegalArgumentException expected but not thrown");
29556         } catch (final IllegalArgumentException ignore) {
29557         }
29558         try {
29559             calibrator.setInitialBias(new Matrix(1, 1));
29560             fail("IllegalArgumentException expected but not thrown");
29561         } catch (final IllegalArgumentException ignore) {
29562         }
29563         try {
29564             calibrator.setInitialBias(new Matrix(3, 3));
29565             fail("IllegalArgumentException expected but not thrown");
29566         } catch (final IllegalArgumentException ignore) {
29567         }
29568     }
29569 
29570     @Test
29571     public void testGetSetInitialMa() throws WrongSizeException, LockedException {
29572         final KnownPositionAccelerometerCalibrator calibrator =
29573                 new KnownPositionAccelerometerCalibrator();
29574 
29575         // test default value
29576         final Matrix ma1 = calibrator.getInitialMa();
29577         assertEquals(ma1, new Matrix(3, 3));
29578 
29579         // set new value
29580         final Matrix ma2 = generateMaGeneral();
29581         calibrator.setInitialMa(ma2);
29582 
29583         // check
29584         final Matrix ma3 = calibrator.getInitialMa();
29585         final Matrix ma4 = new Matrix(3, 3);
29586         calibrator.getInitialMa(ma4);
29587 
29588         assertEquals(ma2, ma3);
29589         assertEquals(ma2, ma4);
29590 
29591         // Force IllegalArgumentException
29592         try {
29593             calibrator.getInitialMa(new Matrix(1, 3));
29594             fail("IllegalArgumentException expected but not thrown");
29595         } catch (final IllegalArgumentException ignore) {
29596         }
29597         try {
29598             calibrator.getInitialMa(new Matrix(3, 1));
29599             fail("IllegalArgumentException expected but not thrown");
29600         } catch (final IllegalArgumentException ignore) {
29601         }
29602         try {
29603             calibrator.setInitialMa(new Matrix(1, 3));
29604             fail("IllegalArgumentException expected but not thrown");
29605         } catch (final IllegalArgumentException ignore) {
29606         }
29607         try {
29608             calibrator.setInitialMa(new Matrix(3, 1));
29609             fail("IllegalArgumentException expected but not thrown");
29610         } catch (final IllegalArgumentException ignore) {
29611         }
29612     }
29613 
29614     @Test
29615     public void testGetSetEcefPosition() throws LockedException {
29616         final KnownPositionAccelerometerCalibrator calibrator =
29617                 new KnownPositionAccelerometerCalibrator();
29618 
29619         // check default value
29620         assertNull(calibrator.getEcefPosition());
29621 
29622         // set new value
29623         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29624         final double latitude = Math.toRadians(
29625                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29626         final double longitude = Math.toRadians(
29627                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29628         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29629         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29630         final NEDVelocity nedVelocity = new NEDVelocity();
29631         final ECEFPosition ecefPosition = new ECEFPosition();
29632         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29633         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29634                 ecefPosition, ecefVelocity);
29635 
29636         calibrator.setPosition(ecefPosition);
29637 
29638         // check
29639         assertSame(calibrator.getEcefPosition(), ecefPosition);
29640     }
29641 
29642     @Test
29643     public void testGetSetNedPosition() throws LockedException {
29644         final KnownPositionAccelerometerCalibrator calibrator =
29645                 new KnownPositionAccelerometerCalibrator();
29646 
29647         // check default value
29648         assertNull(calibrator.getNedPosition());
29649         assertFalse(calibrator.getNedPosition(null));
29650 
29651         // set new value
29652         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29653         final double latitude = Math.toRadians(
29654                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29655         final double longitude = Math.toRadians(
29656                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29657         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29658         final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
29659 
29660         calibrator.setPosition(nedPosition1);
29661 
29662         // check
29663         final NEDPosition nedPosition2 = calibrator.getNedPosition();
29664         final NEDPosition nedPosition3 = new NEDPosition();
29665         calibrator.getNedPosition(nedPosition3);
29666 
29667         assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
29668         assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
29669     }
29670 
29671     @Test
29672     public void testGetSetMeasurements() throws LockedException {
29673         final KnownPositionAccelerometerCalibrator calibrator =
29674                 new KnownPositionAccelerometerCalibrator();
29675 
29676         // check default value
29677         assertNull(calibrator.getMeasurements());
29678 
29679         // set new value
29680         final Collection<StandardDeviationBodyKinematics> measurements =
29681                 Collections.emptyList();
29682         calibrator.setMeasurements(measurements);
29683 
29684         // check
29685         assertSame(calibrator.getMeasurements(), measurements);
29686     }
29687 
29688     @Test
29689     public void testIsSetCommonAxisUsed() throws LockedException {
29690         final KnownPositionAccelerometerCalibrator calibrator =
29691                 new KnownPositionAccelerometerCalibrator();
29692 
29693         // check default value
29694         assertFalse(calibrator.isCommonAxisUsed());
29695 
29696         // set new value
29697         calibrator.setCommonAxisUsed(true);
29698 
29699         // check
29700         assertTrue(calibrator.isCommonAxisUsed());
29701     }
29702 
29703     @Test
29704     public void testGetSetListener() throws LockedException {
29705         final KnownPositionAccelerometerCalibrator calibrator =
29706                 new KnownPositionAccelerometerCalibrator();
29707 
29708         // check default value
29709         assertNull(calibrator.getListener());
29710 
29711         // set new value
29712         calibrator.setListener(this);
29713 
29714         // check
29715         assertSame(calibrator.getListener(), this);
29716     }
29717 
29718     @Test
29719     public void testIsReady() throws LockedException {
29720         final KnownPositionAccelerometerCalibrator calibrator =
29721                 new KnownPositionAccelerometerCalibrator();
29722 
29723         // check
29724         assertFalse(calibrator.isReady());
29725 
29726 
29727         // set empty measurements
29728         final Collection<StandardDeviationBodyKinematics> measurements1 =
29729                 Collections.emptyList();
29730         calibrator.setMeasurements(measurements1);
29731 
29732         // check
29733         assertFalse(calibrator.isReady());
29734 
29735 
29736         // set enough measurements for general case
29737         calibrator.setCommonAxisUsed(false);
29738 
29739         final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
29740         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29741             measurements2.add(new StandardDeviationBodyKinematics());
29742         }
29743         calibrator.setMeasurements(measurements2);
29744 
29745         // check
29746         assertFalse(calibrator.isReady());
29747 
29748 
29749         // set position
29750         final ECEFPosition position = new ECEFPosition();
29751         calibrator.setPosition(position);
29752 
29753         assertTrue(calibrator.isReady());
29754 
29755 
29756         // set enough measurements for common axis case
29757         measurements2.clear();
29758         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
29759             measurements2.add(new StandardDeviationBodyKinematics());
29760         }
29761         calibrator.setMeasurements(measurements2);
29762 
29763         // check
29764         assertFalse(calibrator.isReady());
29765 
29766         calibrator.setCommonAxisUsed(true);
29767 
29768         assertTrue(calibrator.isReady());
29769     }
29770 
29771     @Test
29772     public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
29773             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29774             LockedException, NotReadyException {
29775 
29776         int numValid = 0;
29777         for (int t = 0; t < TIMES; t++) {
29778             final Matrix ba = generateBa();
29779             final Matrix bg = generateBg();
29780             final Matrix ma = generateMaGeneral();
29781             final Matrix mg = generateMg();
29782             final Matrix gg = generateGg();
29783             // when using minimum number of measurements we must not add any noise so that
29784             // a solution is found, when adding more measurements, certain noise can be added
29785             final double accelNoiseRootPSD = 0.0;
29786             final double gyroNoiseRootPSD = 0.0;
29787             final double accelQuantLevel = 0.0;
29788             final double gyroQuantLevel = 0.0;
29789 
29790             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29791                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29792 
29793             final Random random = new Random();
29794             final UniformRandomizer randomizer = new UniformRandomizer(random);
29795             final double latitude = Math.toRadians(
29796                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29797             final double longitude = Math.toRadians(
29798                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29799             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29800             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29801 
29802             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29803             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29804             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29805 
29806             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29807             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
29808                 final double roll = Math.toRadians(
29809                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29810                 final double pitch = Math.toRadians(
29811                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29812                 final double yaw = Math.toRadians(
29813                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29814                 final CoordinateTransformation nedC = new CoordinateTransformation(
29815                         roll, pitch, yaw, FrameType.BODY_FRAME,
29816                         FrameType.LOCAL_NAVIGATION_FRAME);
29817 
29818                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29819                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29820                         .convertNEDtoECEFAndReturnNew(nedFrame);
29821 
29822                 // compute ground-truth kinematics that should be generated at provided
29823                 // position, velocity and orientation
29824                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29825                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29826                                 ecefFrame);
29827 
29828                 // apply known calibration parameters to distort ground-truth and generate a
29829                 // measured kinematics sample
29830                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29831                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29832 
29833                 final StandardDeviationBodyKinematics measurement =
29834                         new StandardDeviationBodyKinematics(measuredKinematics,
29835                                 specificForceStandardDeviation,
29836                                 angularRateStandardDeviation);
29837                 measurements.add(measurement);
29838             }
29839 
29840             // When we have the minimum number of measurements, we need to provide
29841             // an initial solution close to the true solution
29842             final KnownPositionAccelerometerCalibrator calibrator =
29843                     new KnownPositionAccelerometerCalibrator(nedPosition,
29844                             measurements, false, ba, ma, this);
29845 
29846             // estimate
29847             reset();
29848             assertTrue(calibrator.isReady());
29849             assertFalse(calibrator.isRunning());
29850             assertEquals(mCalibrateStart, 0);
29851             assertEquals(mCalibrateEnd, 0);
29852 
29853             try {
29854                 calibrator.calibrate();
29855             } catch (final CalibrationException e) {
29856                 continue;
29857             }
29858 
29859             // check
29860             assertTrue(calibrator.isReady());
29861             assertFalse(calibrator.isRunning());
29862             assertEquals(mCalibrateStart, 1);
29863             assertEquals(mCalibrateEnd, 1);
29864 
29865             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
29866             final Matrix estimatedMa = calibrator.getEstimatedMa();
29867 
29868             // since we have used the minimum number of measurements,
29869             // result is quite unstable (calibration exception is usually thrown)
29870             // and inaccurate.
29871             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
29872                 continue;
29873             }
29874             if (!ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR)) {
29875                 continue;
29876             }
29877 
29878             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
29879             assertTrue(ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR));
29880 
29881             numValid++;
29882 
29883             break;
29884         }
29885 
29886         assertTrue(numValid > 0);
29887     }
29888 
29889     @Test
29890     public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
29891             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
29892             LockedException, NotReadyException {
29893 
29894         int numValid = 0;
29895         for (int t = 0; t < TIMES; t++) {
29896             final Matrix ba = generateBa();
29897             final Matrix bg = generateBg();
29898             final Matrix ma = generateMaGeneral();
29899             final Matrix mg = generateMg();
29900             final Matrix gg = generateGg();
29901             // when using minimum number of measurements we must not add any noise so that
29902             // a solution is found, when adding more measurements, certain noise can be added
29903             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
29904             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
29905             final double accelQuantLevel = 0.0;
29906             final double gyroQuantLevel = 0.0;
29907 
29908             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
29909                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
29910 
29911             final Random random = new Random();
29912             final UniformRandomizer randomizer = new UniformRandomizer(random);
29913             final double latitude = Math.toRadians(
29914                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29915             final double longitude = Math.toRadians(
29916                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29917             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29918             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29919 
29920             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29921             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29922             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29923 
29924             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
29925             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29926                 final double roll = Math.toRadians(
29927                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29928                 final double pitch = Math.toRadians(
29929                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29930                 final double yaw = Math.toRadians(
29931                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29932                 final CoordinateTransformation nedC = new CoordinateTransformation(
29933                         roll, pitch, yaw, FrameType.BODY_FRAME,
29934                         FrameType.LOCAL_NAVIGATION_FRAME);
29935 
29936                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
29937                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
29938                         .convertNEDtoECEFAndReturnNew(nedFrame);
29939 
29940                 // compute ground-truth kinematics that should be generated at provided
29941                 // position, velocity and orientation
29942                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
29943                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
29944                                 ecefFrame);
29945 
29946                 // apply known calibration parameters to distort ground-truth and generate a
29947                 // measured kinematics sample
29948                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
29949                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
29950 
29951                 final StandardDeviationBodyKinematics measurement =
29952                         new StandardDeviationBodyKinematics(measuredKinematics,
29953                                 specificForceStandardDeviation,
29954                                 angularRateStandardDeviation);
29955                 measurements.add(measurement);
29956             }
29957 
29958             // When we have the minimum number of measurements, we need to provide
29959             // an initial solution close to the true solution
29960             final KnownPositionAccelerometerCalibrator calibrator =
29961                     new KnownPositionAccelerometerCalibrator(nedPosition,
29962                             measurements, false, ba, ma, this);
29963 
29964             // estimate
29965             reset();
29966             assertTrue(calibrator.isReady());
29967             assertFalse(calibrator.isRunning());
29968             assertEquals(mCalibrateStart, 0);
29969             assertEquals(mCalibrateEnd, 0);
29970 
29971             try {
29972                 calibrator.calibrate();
29973             } catch (final CalibrationException e) {
29974                 continue;
29975             }
29976 
29977             // check
29978             assertTrue(calibrator.isReady());
29979             assertFalse(calibrator.isRunning());
29980             assertEquals(mCalibrateStart, 1);
29981             assertEquals(mCalibrateEnd, 1);
29982 
29983             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
29984             final Matrix estimatedMa = calibrator.getEstimatedMa();
29985 
29986             // since we have used the minimum number of measurements,
29987             // result is quite unstable (calibration exception is usually thrown)
29988             // and inaccurate.
29989             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
29990                 continue;
29991             }
29992             if (!ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR)) {
29993                 continue;
29994             }
29995 
29996             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
29997             assertTrue(ma.equals(estimatedMa, 3.0 * LARGE_ABSOLUTE_ERROR));
29998 
29999             numValid++;
30000 
30001             break;
30002         }
30003 
30004         assertTrue(numValid > 0);
30005     }
30006 
30007 
30008     @Test
30009     public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
30010             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
30011             LockedException, NotReadyException {
30012 
30013         int numValid = 0;
30014         for (int t = 0; t < TIMES; t++) {
30015             final Matrix ba = generateBa();
30016             final Matrix bg = generateBg();
30017             final Matrix ma = generateMaCommonAxis();
30018             final Matrix mg = generateMg();
30019             final Matrix gg = generateGg();
30020             // when using minimum number of measurements we must not add any noise so that
30021             // a solution is found, when adding more measurements, certain noise can be added
30022             final double accelNoiseRootPSD = 0.0;
30023             final double gyroNoiseRootPSD = 0.0;
30024             final double accelQuantLevel = 0.0;
30025             final double gyroQuantLevel = 0.0;
30026 
30027             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
30028                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
30029 
30030             final Random random = new Random();
30031             final UniformRandomizer randomizer = new UniformRandomizer(random);
30032             final double latitude = Math.toRadians(
30033                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30034             final double longitude = Math.toRadians(
30035                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30036             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30037             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30038 
30039             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
30040             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
30041             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
30042 
30043             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
30044             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
30045                 final double roll = Math.toRadians(
30046                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30047                 final double pitch = Math.toRadians(
30048                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30049                 final double yaw = Math.toRadians(
30050                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30051                 final CoordinateTransformation nedC = new CoordinateTransformation(
30052                         roll, pitch, yaw, FrameType.BODY_FRAME,
30053                         FrameType.LOCAL_NAVIGATION_FRAME);
30054 
30055                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30056                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30057                         .convertNEDtoECEFAndReturnNew(nedFrame);
30058 
30059                 // compute ground-truth kinematics that should be generated at provided
30060                 // position, velocity and orientation
30061                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30062                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30063                                 ecefFrame);
30064 
30065                 // apply known calibration parameters to distort ground-truth and generate a
30066                 // measured kinematics sample
30067                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30068                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30069 
30070                 final StandardDeviationBodyKinematics measurement =
30071                         new StandardDeviationBodyKinematics(measuredKinematics,
30072                                 specificForceStandardDeviation,
30073                                 angularRateStandardDeviation);
30074                 measurements.add(measurement);
30075             }
30076 
30077             // When we have the minimum number of measurements, we need to provide
30078             // an initial solution close to the true solution
30079             final KnownPositionAccelerometerCalibrator calibrator =
30080                     new KnownPositionAccelerometerCalibrator(nedPosition,
30081                             measurements, true, ba, ma, this);
30082 
30083             // estimate
30084             reset();
30085             assertTrue(calibrator.isReady());
30086             assertFalse(calibrator.isRunning());
30087             assertEquals(mCalibrateStart, 0);
30088             assertEquals(mCalibrateEnd, 0);
30089 
30090             try {
30091                 calibrator.calibrate();
30092             } catch (final CalibrationException e) {
30093                 continue;
30094             }
30095 
30096             // check
30097             assertTrue(calibrator.isReady());
30098             assertFalse(calibrator.isRunning());
30099             assertEquals(mCalibrateStart, 1);
30100             assertEquals(mCalibrateEnd, 1);
30101 
30102             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
30103             final Matrix estimatedMa = calibrator.getEstimatedMa();
30104 
30105             // since we have used the minimum number of measurements,
30106             // result is quite unstable (calibration exception is usually thrown)
30107             // and inaccurate.
30108             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
30109                 continue;
30110             }
30111             if (!ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR)) {
30112                 continue;
30113             }
30114 
30115             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
30116             assertTrue(ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR));
30117 
30118             numValid++;
30119 
30120             break;
30121         }
30122 
30123         assertTrue(numValid > 0);
30124     }
30125 
30126     @Test
30127     public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
30128             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
30129             LockedException, NotReadyException {
30130 
30131         int numValid = 0;
30132         for (int t = 0; t < TIMES; t++) {
30133             final Matrix ba = generateBa();
30134             final Matrix bg = generateBg();
30135             final Matrix ma = generateMaCommonAxis();
30136             final Matrix mg = generateMg();
30137             final Matrix gg = generateGg();
30138             // when using minimum number of measurements we must not add any noise so that
30139             // a solution is found, when adding more measurements, certain noise can be added
30140             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
30141             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
30142             final double accelQuantLevel = 0.0;
30143             final double gyroQuantLevel = 0.0;
30144 
30145             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
30146                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
30147 
30148             final Random random = new Random();
30149             final UniformRandomizer randomizer = new UniformRandomizer(random);
30150             final double latitude = Math.toRadians(
30151                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30152             final double longitude = Math.toRadians(
30153                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30154             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30155             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30156 
30157             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
30158             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
30159             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
30160 
30161             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
30162             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
30163                 final double roll = Math.toRadians(
30164                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30165                 final double pitch = Math.toRadians(
30166                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30167                 final double yaw = Math.toRadians(
30168                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30169                 final CoordinateTransformation nedC = new CoordinateTransformation(
30170                         roll, pitch, yaw, FrameType.BODY_FRAME,
30171                         FrameType.LOCAL_NAVIGATION_FRAME);
30172 
30173                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30174                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30175                         .convertNEDtoECEFAndReturnNew(nedFrame);
30176 
30177                 // compute ground-truth kinematics that should be generated at provided
30178                 // position, velocity and orientation
30179                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30180                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30181                                 ecefFrame);
30182 
30183                 // apply known calibration parameters to distort ground-truth and generate a
30184                 // measured kinematics sample
30185                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
30186                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
30187 
30188                 final StandardDeviationBodyKinematics measurement =
30189                         new StandardDeviationBodyKinematics(measuredKinematics,
30190                                 specificForceStandardDeviation,
30191                                 angularRateStandardDeviation);
30192                 measurements.add(measurement);
30193             }
30194 
30195             // When we have the minimum number of measurements, we need to provide
30196             // an initial solution close to the true solution
30197             final KnownPositionAccelerometerCalibrator calibrator =
30198                     new KnownPositionAccelerometerCalibrator(nedPosition,
30199                             measurements, true, ba, ma, this);
30200 
30201             // estimate
30202             reset();
30203             assertTrue(calibrator.isReady());
30204             assertFalse(calibrator.isRunning());
30205             assertEquals(mCalibrateStart, 0);
30206             assertEquals(mCalibrateEnd, 0);
30207 
30208             try {
30209                 calibrator.calibrate();
30210             } catch (final CalibrationException e) {
30211                 continue;
30212             }
30213 
30214             // check
30215             assertTrue(calibrator.isReady());
30216             assertFalse(calibrator.isRunning());
30217             assertEquals(mCalibrateStart, 1);
30218             assertEquals(mCalibrateEnd, 1);
30219 
30220             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
30221             final Matrix estimatedMa = calibrator.getEstimatedMa();
30222 
30223             // since we have used the minimum number of measurements,
30224             // result is quite unstable (calibration exception is usually thrown)
30225             // and inaccurate.
30226             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
30227                 continue;
30228             }
30229             if (!ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR)) {
30230                 continue;
30231             }
30232 
30233             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
30234             assertTrue(ma.equals(estimatedMa, 20.0 * LARGE_ABSOLUTE_ERROR));
30235 
30236             numValid++;
30237 
30238             break;
30239         }
30240 
30241         assertTrue(numValid > 0);
30242     }
30243 
30244     @Ignore
30245     @Test
30246     public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
30247 
30248         final Random random = new Random();
30249         final UniformRandomizer randomizer = new UniformRandomizer(random);
30250         final double latitude = Math.toRadians(
30251                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30252         final double longitude = Math.toRadians(
30253                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30254         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30255         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30256 
30257         final double roll = Math.toRadians(
30258                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30259         final double pitch = Math.toRadians(
30260                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30261         final double yaw = Math.toRadians(
30262                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
30263         final CoordinateTransformation nedC = new CoordinateTransformation(
30264                 roll, pitch, yaw, FrameType.BODY_FRAME,
30265                 FrameType.LOCAL_NAVIGATION_FRAME);
30266 
30267         final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
30268         final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
30269                 .convertNEDtoECEFAndReturnNew(nedFrame);
30270 
30271         final BodyKinematics trueKinematics = ECEFKinematicsEstimator
30272                 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
30273                         ecefFrame);
30274 
30275         final double fNorm = trueKinematics.getSpecificForceNorm();
30276         final double wNorm = trueKinematics.getAngularRateNorm();
30277 
30278         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
30279         final double g = gravity.getNorm();
30280 
30281         assertEquals(g, fNorm, ABSOLUTE_ERROR);
30282         assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
30283     }
30284 
30285     @Override
30286     public void onCalibrateStart(KnownPositionAccelerometerCalibrator calibrator) {
30287         checkLocked(calibrator);
30288         mCalibrateStart++;
30289     }
30290 
30291     @Override
30292     public void onCalibrateEnd(KnownPositionAccelerometerCalibrator calibrator) {
30293         checkLocked(calibrator);
30294         mCalibrateEnd++;
30295     }
30296 
30297     private void reset() {
30298         mCalibrateStart = 0;
30299         mCalibrateEnd = 0;
30300     }
30301 
30302     private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
30303         assertTrue(calibrator.isRunning());
30304         try {
30305             calibrator.setInitialBiasX(0.0);
30306             fail("LockedException expected but not thrown");
30307         } catch (final LockedException ignore) {
30308         }
30309         try {
30310             calibrator.setInitialBiasY(0.0);
30311             fail("LockedException expected but not thrown");
30312         } catch (final LockedException ignore) {
30313         }
30314         try {
30315             calibrator.setInitialBiasZ(0.0);
30316             fail("LockedException expected but not thrown");
30317         } catch (final LockedException ignore) {
30318         }
30319         try {
30320             calibrator.setInitialBiasX(null);
30321             fail("LockedException expected but not thrown");
30322         } catch (final LockedException ignore) {
30323         }
30324         try {
30325             calibrator.setInitialBiasY(null);
30326             fail("LockedException expected but not thrown");
30327         } catch (final LockedException ignore) {
30328         }
30329         try {
30330             calibrator.setInitialBiasZ(null);
30331             fail("LockedException expected but not thrown");
30332         } catch (final LockedException ignore) {
30333         }
30334         try {
30335             calibrator.setInitialBias(0.0, 0.0, 0.0);
30336             fail("LockedException expected but not thrown");
30337         } catch (final LockedException ignore) {
30338         }
30339         try {
30340             calibrator.setInitialBias(null, null, null);
30341             fail("LockedException expected but not thrown");
30342         } catch (final LockedException ignore) {
30343         }
30344         try {
30345             calibrator.setInitialSx(0.0);
30346             fail("LockedException expected but not thrown");
30347         } catch (final LockedException ignore) {
30348         }
30349         try {
30350             calibrator.setInitialSy(0.0);
30351             fail("LockedException expected but not thrown");
30352         } catch (final LockedException ignore) {
30353         }
30354         try {
30355             calibrator.setInitialSz(0.0);
30356             fail("LockedException expected but not thrown");
30357         } catch (final LockedException ignore) {
30358         }
30359         try {
30360             calibrator.setInitialMxy(0.0);
30361             fail("LockedException expected but not thrown");
30362         } catch (final LockedException ignore) {
30363         }
30364         try {
30365             calibrator.setInitialMxz(0.0);
30366             fail("LockedException expected but not thrown");
30367         } catch (final LockedException ignore) {
30368         }
30369         try {
30370             calibrator.setInitialMyx(0.0);
30371             fail("LockedException expected but not thrown");
30372         } catch (final LockedException ignore) {
30373         }
30374         try {
30375             calibrator.setInitialMyz(0.0);
30376             fail("LockedException expected but not thrown");
30377         } catch (final LockedException ignore) {
30378         }
30379         try {
30380             calibrator.setInitialMzx(0.0);
30381             fail("LockedException expected but not thrown");
30382         } catch (final LockedException ignore) {
30383         }
30384         try {
30385             calibrator.setInitialMzy(0.0);
30386             fail("LockedException expected but not thrown");
30387         } catch (final LockedException ignore) {
30388         }
30389         try {
30390             calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
30391             fail("LockedException expected but not thrown");
30392         } catch (final LockedException ignore) {
30393         }
30394         try {
30395             calibrator.setInitialCrossCouplingErrors(
30396                     0.0, 0.0, 0.0,
30397                     0.0, 0.0, 0.0);
30398             fail("LockedException expected but not thrown");
30399         } catch (final LockedException ignore) {
30400         }
30401         try {
30402             calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
30403                     0.0, 0.0, 0.0,
30404                     0.0, 0.0, 0.0,
30405                     0.0, 0.0, 0.0);
30406             fail("LockedException expected but not thrown");
30407         } catch (final LockedException ignore) {
30408         }
30409         try {
30410             calibrator.setInitialBias((double[]) null);
30411             fail("LockedException expected but not thrown");
30412         } catch (final LockedException ignore) {
30413         }
30414         try {
30415             calibrator.setInitialBias((Matrix) null);
30416             fail("LockedException expected but not thrown");
30417         } catch (final LockedException ignore) {
30418         }
30419         try {
30420             calibrator.setInitialMa(null);
30421             fail("LockedException expected but not thrown");
30422         } catch (final LockedException ignore) {
30423         }
30424         try {
30425             calibrator.setPosition((ECEFPosition) null);
30426             fail("LockedException expected but not thrown");
30427         } catch (final LockedException ignore) {
30428         }
30429         try {
30430             calibrator.setPosition((NEDPosition) null);
30431             fail("LockedException expected but not thrown");
30432         } catch (final LockedException ignore) {
30433         }
30434         try {
30435             calibrator.setMeasurements(null);
30436             fail("LockedException expected but not thrown");
30437         } catch (final LockedException ignore) {
30438         }
30439         try {
30440             calibrator.setCommonAxisUsed(true);
30441             fail("LockedException expected but not thrown");
30442         } catch (final LockedException ignore) {
30443         }
30444         try {
30445             calibrator.setListener(this);
30446             fail("LockedException expected but not thrown");
30447         } catch (final LockedException ignore) {
30448         }
30449         try {
30450             calibrator.calibrate();
30451             fail("LockedException expected but not thrown");
30452         } catch (final LockedException ignore) {
30453         } catch (final Exception e) {
30454             fail("LockedException expected but not thrown");
30455         }
30456     }
30457 
30458     private Matrix generateBa() {
30459         return Matrix.newFromArray(new double[]{
30460                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30461                 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
30462                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
30463     }
30464 
30465     private Matrix generateBg() {
30466         return Matrix.newFromArray(new double[]{
30467                 -9 * DEG_TO_RAD / 3600.0,
30468                 13 * DEG_TO_RAD / 3600.0,
30469                 -8 * DEG_TO_RAD / 3600.0});
30470     }
30471 
30472     private Matrix generateMaGeneral() throws WrongSizeException {
30473         final Matrix result = new Matrix(3, 3);
30474         result.fromArray(new double[]{
30475                 500e-6, -300e-6, 200e-6,
30476                 -150e-6, -600e-6, 250e-6,
30477                 -250e-6, 100e-6, 450e-6
30478         }, false);
30479 
30480         return result;
30481     }
30482 
30483     private Matrix generateMaCommonAxis() throws WrongSizeException {
30484         final Matrix result = new Matrix(3, 3);
30485         result.fromArray(new double[]{
30486                 500e-6, -300e-6, 200e-6,
30487                 0.0, -600e-6, 250e-6,
30488                 0.0, 0.0, 450e-6
30489         }, false);
30490 
30491         return result;
30492     }
30493 
30494     private Matrix generateMg() throws WrongSizeException {
30495         final Matrix result = new Matrix(3, 3);
30496         result.fromArray(new double[]{
30497                 400e-6, -300e-6, 250e-6,
30498                 0.0, -300e-6, -150e-6,
30499                 0.0, 0.0, -350e-6
30500         }, false);
30501 
30502         return result;
30503     }
30504 
30505     private Matrix generateGg() throws WrongSizeException {
30506         final Matrix result = new Matrix(3, 3);
30507         final double tmp = DEG_TO_RAD / (3600 * 9.80665);
30508         result.fromArray(new double[]{
30509                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
30510                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
30511                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
30512         }, false);
30513 
30514         return result;
30515     }
30516 
30517     private double getAccelNoiseRootPSD() {
30518         return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
30519     }
30520 
30521     private double getGyroNoiseRootPSD() {
30522         return 0.01 * DEG_TO_RAD / 60.0;
30523     }
30524 }